Class: Bullet3::Simulation

Inherits:
Object
  • Object
show all
Defined in:
lib/bullet3/simulation.rb

Instance Attribute Summary collapse

Instance Method Summary collapse

Constructor Details

#initialize(mode: :direct) ⇒ Simulation

Returns a new instance of Simulation.

Raises:

  • (ArgumentError) 


10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
 
# File 'lib/bullet3/simulation.rb', line 10

def initialize(mode: :direct)
  mode = mode.to_sym
  raise ArgumentError, "unsupported connection mode: #{mode}" unless %i[direct gui shared_memory].include?(mode)
  Bullet3.require_native_extension!("Bullet3::Simulation") unless Bullet3.native?

  @mode = mode
  @world = DiscreteDynamicsWorld.create
  @shapes = []
  @shape_specs = []
  @bodies = []
  @body_specs = []
  @constraints = []
  @constraint_specs = []
  @models = {}
  @importers = []
end

Instance Attribute Details

#mode ⇒ Object (readonly)

Returns the value of attribute mode.



8
9
10
 
# File 'lib/bullet3/simulation.rb', line 8

def mode
  @mode
end

#world ⇒ Object (readonly)

Returns the value of attribute world.



8
9
10
 
# File 'lib/bullet3/simulation.rb', line 8

def world
  @world
end

Instance Method Details

#body(body_id) ⇒ Object 



303
304
305
 
# File 'lib/bullet3/simulation.rb', line 303

def body(body_id)
  body_for(body_id)
end

#change_dynamics(body_id, lateral_friction: nil, restitution: nil) ⇒ Object 



161
162
163
164
165
166
 
# File 'lib/bullet3/simulation.rb', line 161

def change_dynamics(body_id, lateral_friction: nil, restitution: nil)
  body = body_for(body_id)
  body.friction = Float(lateral_friction) unless lateral_friction.nil?
  body.restitution = Float(restitution) unless restitution.nil?
  nil
end

#collision_shape(shape_id) ⇒ Object 



307
308
309
 
# File 'lib/bullet3/simulation.rb', line 307

def collision_shape(shape_id)
  shape_for(shape_id)
end

#constraint(constraint_id) ⇒ Object 



311
312
313
 
# File 'lib/bullet3/simulation.rb', line 311

def constraint(constraint_id)
  constraint_for(constraint_id)
end

#create_collision_shape(type, **options) ⇒ Object 



36
37
38
39
 
# File 'lib/bullet3/simulation.rb', line 36

def create_collision_shape(type, **options)
  shape = build_collision_shape(type.to_sym, options)
  register_shape(shape, type: type.to_sym, options: serializable_options(options))
end

#create_constraint(type, **options) ⇒ Object 



62
63
64
65
66
67
68
69
 
# File 'lib/bullet3/simulation.rb', line 62

def create_constraint(type, **options)
  constraint = build_constraint(type.to_sym, options)
  world.add_constraint(constraint, options.fetch(:disable_collisions_between_linked_bodies, false))

  @constraints << constraint
  @constraint_specs << { type: type.to_sym, options: serializable_options(options) }
  @constraints.length - 1
end

#create_rigid_body(mass:, collision_shape:, position: [0, 0, 0], orientation: Quaternion.identity, rgba_color: [220, 220, 220, 255]) ⇒ Object 



41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
 
# File 'lib/bullet3/simulation.rb', line 41

def create_rigid_body(mass:, collision_shape:, position: [0, 0, 0], orientation: Quaternion.identity, rgba_color: [220, 220, 220, 255])
  shape = shape_for(collision_shape)
  position_vector = Vector3.coerce(position)
  orientation_quaternion = Quaternion.coerce(orientation)
  transform = Transform.new(orientation_quaternion, position_vector)
  motion_state = MotionState.new(transform)
  info = RigidBodyConstructionInfo.new(Float(mass), motion_state, shape)
  body = RigidBody.new(info)
  world.add_rigid_body(body)

  @bodies << body
  @body_specs << {
    mass: Float(mass),
    collision_shape: Integer(collision_shape),
    position: position_vector.to_a,
    orientation: orientation_quaternion.to_a,
    rgba_color: rgba_color_argument(rgba_color)
  }
  @bodies.length - 1
end

#debug_draw_world(drawer = DebugDraw.new, mode: nil) ⇒ Object 



230
231
232
233
234
235
236
237
238
239
240
241
 
# File 'lib/bullet3/simulation.rb', line 230

def debug_draw_world(drawer = DebugDraw.new, mode: nil)
  drawer.debug_mode = Integer(mode) if mode

  if world.respond_to?(:debug_drawer=) && world.respond_to?(:debug_draw_world)
    world.debug_drawer = drawer
    world.debug_draw_world
  else
    draw_world_fallback(drawer)
  end

  drawer
end

#disconnect ⇒ Object 



331
332
333
334
335
336
337
338
339
340
341
342
343
 
# File 'lib/bullet3/simulation.rb', line 331

def disconnect
  clear_importers
  @constraints.compact.each { |constraint| world.remove_constraint(constraint) }
  @bodies.compact.each { |body| world.remove_rigid_body(body) }
  @constraints.clear
  @constraint_specs.clear
  @bodies.clear
  @body_specs.clear
  @shapes.clear
  @shape_specs.clear
  @models.clear
  nil
end

#get_aabb(body_id) ⇒ Object 



127
128
129
130
 
# File 'lib/bullet3/simulation.rb', line 127

def get_aabb(body_id)
  body = body_for(body_id)
  body.collision_shape.aabb(body.world_transform).map(&:to_a)
end

#get_base_position_and_orientation(body_id) ⇒ Object 



116
117
118
119
 
# File 'lib/bullet3/simulation.rb', line 116

def get_base_position_and_orientation(body_id)
  transform = body_for(body_id).world_transform
  [transform.origin.to_a, transform.rotation.to_a]
end

#get_camera_image(width, height, camera_eye_position: [0, 0, 5], camera_target_position: [0, 0, 0], camera_up_vector: [0, 1, 0], fov: 60.0, near: 0.01, far: 100.0, background_color: [0, 0, 0, 255], body_colors: {}) ⇒ Object

Raises:

  • (ArgumentError) 


208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
 
# File 'lib/bullet3/simulation.rb', line 208

def get_camera_image(width, height, camera_eye_position: [0, 0, 5], camera_target_position: [0, 0, 0], camera_up_vector: [0, 1, 0], fov: 60.0, near: 0.01, far: 100.0, background_color: [0, 0, 0, 255], body_colors: {})
  width = Integer(width)
  height = Integer(height)
  raise ArgumentError, "width and height must be positive" unless width.positive? && height.positive?

  camera = camera_basis(camera_eye_position, camera_target_position, camera_up_vector, Float(fov), Float(far))
  background_color = rgba_color_argument(background_color)
  body_colors = body_colors.to_h { |body_id, color| [Integer(body_id), rgba_color_argument(color)] }
  rgb = []
  depth = []
  segmentation = []

  height.times do |row|
    width.times do |column|
      hit = camera_ray_hit(camera, column, row, width, height)
      append_camera_pixel(hit, rgb, depth, segmentation, background_color, body_colors)
    end
  end

  [width, height, rgb, depth, segmentation]
end

#get_contact_points(body_a: nil, body_b: nil) ⇒ Object 



147
148
149
150
151
152
153
154
155
156
157
158
159
 
# File 'lib/bullet3/simulation.rb', line 147

def get_contact_points(body_a: nil, body_b: nil)
  contacts = if body_a && body_b
               world.contact_pair_test(body_for(body_a), body_for(body_b))
             elsif body_a
               world.contact_test(body_for(body_a))
             elsif body_b
               world.contact_test(body_for(body_b))
             else
               manifold_contacts
             end

  contacts.map { |contact| contact_with_body_ids(contact) }
end

#get_joint_info(body_id, joint_index) ⇒ Object 



172
173
174
 
# File 'lib/bullet3/simulation.rb', line 172

def get_joint_info(body_id, joint_index)
  model_for(body_id)[:joints].fetch(Integer(joint_index)) { raise ArgumentError, "unknown joint index: #{joint_index}" }.dup
end

#get_joint_state(body_id, joint_index) ⇒ Object 



176
177
178
179
180
181
182
183
184
185
186
 
# File 'lib/bullet3/simulation.rb', line 176

def get_joint_state(body_id, joint_index)
  joint = get_joint_info(body_id, joint_index)
  constraint = joint[:constraint_id] && constraint_for(joint[:constraint_id])
  position = joint_position(joint, constraint)
  {
    position: position,
    velocity: joint[:target_velocity] || 0.0,
    reaction_forces: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    applied_torque: joint[:applied_torque] || 0.0
  }
end

#get_num_joints(body_id) ⇒ Object 



168
169
170
 
# File 'lib/bullet3/simulation.rb', line 168

def get_num_joints(body_id)
  model_for(body_id)[:joints].length
end

#load_bullet(filename) ⇒ Object

Raises:



278
279
280
281
282
283
284
285
286
 
# File 'lib/bullet3/simulation.rb', line 278

def load_bullet(filename)
  raise Bullet3::Error, "native extension is required for .bullet import" unless IO.const_defined?(:BulletWorldImporter, false)

  importer = IO::BulletWorldImporter.new(world)
  raise Bullet3::Error, "failed to load .bullet file: #{filename}" unless importer.load_file(filename)

  @importers << importer
  importer
end

#load_mjcf(filename, base_position: [0, 0, 0], base_orientation: Quaternion.identity, global_scaling: 1.0) ⇒ Object

Raises:

  • (ArgumentError) 


104
105
106
107
108
109
110
111
112
113
114
 
# File 'lib/bullet3/simulation.rb', line 104

def load_mjcf(filename, base_position: [0, 0, 0], base_orientation: Quaternion.identity, global_scaling: 1.0)
  path = Data.find(filename)
  document = REXML::Document.new(File.read(path))
  body_elements = REXML::XPath.match(document, "/mujoco/worldbody/body")
  raise ArgumentError, "MJCF must contain at least one worldbody body: #{filename}" if body_elements.empty?

  base_transform = Transform.new(Quaternion.coerce(base_orientation), Vector3.coerce(base_position))
  body_elements.flat_map do |body|
    load_mjcf_body(body, base_transform, Float(global_scaling), File.dirname(path))
  end
end

#load_sdf(filename, base_position: [0, 0, 0], base_orientation: Quaternion.identity, global_scaling: 1.0) ⇒ Object

Raises:

  • (ArgumentError) 


93
94
95
96
97
98
99
100
101
102
 
# File 'lib/bullet3/simulation.rb', line 93

def load_sdf(filename, base_position: [0, 0, 0], base_orientation: Quaternion.identity, global_scaling: 1.0)
  path = Data.find(filename)
  document = REXML::Document.new(File.read(path))
  model_elements = REXML::XPath.match(document, "//model")
  raise ArgumentError, "SDF must contain at least one model: #{filename}" if model_elements.empty?

  model_elements.flat_map do |model|
    load_sdf_model(model, base_position, base_orientation, Float(global_scaling), File.dirname(path))
  end
end

#load_urdf(filename, base_position: [0, 0, 0], base_orientation: Quaternion.identity, use_fixed_base: false, global_scaling: 1.0) ⇒ Object

Raises:

  • (ArgumentError) 


71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
 
# File 'lib/bullet3/simulation.rb', line 71

def load_urdf(filename, base_position: [0, 0, 0], base_orientation: Quaternion.identity, use_fixed_base: false, global_scaling: 1.0)
  path = Data.find(filename)
  document = REXML::Document.new(File.read(path))
  links = REXML::XPath.match(document, "/robot/link")
  joints = REXML::XPath.match(document, "/robot/joint")
  raise ArgumentError, "URDF must contain at least one link: #{filename}" if links.empty?
  return load_multilink_urdf(links, joints, base_position, base_orientation, use_fixed_base, Float(global_scaling), File.dirname(path)) if links.length > 1 || joints.any?

  link = links.first
  mass = use_fixed_base ? 0.0 : urdf_link_mass(link)
  shape = urdf_collision_shape(link, Float(global_scaling), File.dirname(path))
  shape_id = register_shape(shape, urdf_shape_spec(link, Float(global_scaling), File.dirname(path)))
  body_id = create_rigid_body(
    mass: mass,
    collision_shape: shape_id,
    position: base_position,
    orientation: base_orientation
  )
  apply_urdf_contact(link, body_for(body_id))
  body_id
end

#load_world(filename) ⇒ Object 



253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
 
# File 'lib/bullet3/simulation.rb', line 253

def load_world(filename)
  data = JSON.parse(File.read(filename), symbolize_names: true)
  reset_simulation

  data.fetch(:shapes).each { |shape| create_collision_shape(shape.fetch(:type), **symbolize_hash(shape.fetch(:options))) }
  data.fetch(:bodies).each do |body|
    next if body.nil?

    create_rigid_body(
      mass: body.fetch(:mass),
      collision_shape: body.fetch(:collision_shape),
      position: body.fetch(:position),
      orientation: body.fetch(:orientation),
      rgba_color: body.fetch(:rgba_color, [220, 220, 220, 255])
    )
  end
  data.fetch(:constraints, []).each do |constraint|
    next if constraint.nil?

    create_constraint(constraint.fetch(:type), **symbolize_hash(constraint.fetch(:options)))
  end

  nil
end

#ray_test(from, to) ⇒ Object 



132
133
134
 
# File 'lib/bullet3/simulation.rb', line 132

def ray_test(from, to)
  world.ray_test(from, to)
end

#ray_test_all(from, to) ⇒ Object 



136
137
138
 
# File 'lib/bullet3/simulation.rb', line 136

def ray_test_all(from, to)
  world.ray_test_all(from, to)
end

#ray_test_batch(rays) ⇒ Object 



140
141
142
143
144
145
 
# File 'lib/bullet3/simulation.rb', line 140

def ray_test_batch(rays)
  rays.map do |ray|
    from, to = ray
    ray_test(from, to)
  end
end

#remove_body(body_id) ⇒ Object 



315
316
317
318
319
320
321
 
# File 'lib/bullet3/simulation.rb', line 315

def remove_body(body_id)
  body = body_for(body_id)
  world.remove_rigid_body(body)
  @bodies[Integer(body_id)] = nil
  @body_specs[Integer(body_id)] = nil
  nil
end

#remove_constraint(constraint_id) ⇒ Object 



323
324
325
326
327
328
329
 
# File 'lib/bullet3/simulation.rb', line 323

def remove_constraint(constraint_id)
  constraint = constraint_for(constraint_id)
  world.remove_constraint(constraint)
  @constraints[Integer(constraint_id)] = nil
  @constraint_specs[Integer(constraint_id)] = nil
  nil
end

#reset_simulation ⇒ Object 



288
289
290
291
292
293
294
295
296
297
298
299
300
301
 
# File 'lib/bullet3/simulation.rb', line 288

def reset_simulation
  clear_importers
  @constraints.compact.each { |constraint| world.remove_constraint(constraint) }
  @bodies.compact.each { |body| world.remove_rigid_body(body) }
  @world = DiscreteDynamicsWorld.create
  @bodies.clear
  @body_specs.clear
  @shapes.clear
  @shape_specs.clear
  @constraints.clear
  @constraint_specs.clear
  @models.clear
  nil
end

#save_bullet(filename) ⇒ Object 



248
249
250
251
 
# File 'lib/bullet3/simulation.rb', line 248

def save_bullet(filename)
  serializer = IO::Serializer.new(self)
  serializer.save_bullet(self, filename)
end

#save_world(filename) ⇒ Object 



243
244
245
246
 
# File 'lib/bullet3/simulation.rb', line 243

def save_world(filename)
  File.write(filename, JSON.pretty_generate(world_snapshot))
  filename
end

#set_base_position_and_orientation(body_id, position, orientation = Quaternion.identity) ⇒ Object 



121
122
123
124
125
 
# File 'lib/bullet3/simulation.rb', line 121

def set_base_position_and_orientation(body_id, position, orientation = Quaternion.identity)
  body = body_for(body_id)
  body.world_transform = Transform.new(Quaternion.coerce(orientation), Vector3.coerce(position))
  nil
end

#set_gravity(x, y = nil, z = nil) ⇒ Object 



27
28
29
30
 
# File 'lib/bullet3/simulation.rb', line 27

def set_gravity(x, y = nil, z = nil)
  vector = vector_argument(x, y, z)
  world.gravity = vector
end

#set_joint_motor_control(body_id, joint_index, control_mode:, target_position: nil, target_velocity: 0.0, force: 0.0) ⇒ Object 



188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
 
# File 'lib/bullet3/simulation.rb', line 188

def set_joint_motor_control(body_id, joint_index, control_mode:, target_position: nil, target_velocity: 0.0, force: 0.0)
  model = model_for(body_id)
  joint = model[:joints].fetch(Integer(joint_index)) { raise ArgumentError, "unknown joint index: #{joint_index}" }
  constraint = joint[:constraint_id] && constraint_for(joint[:constraint_id])
  mode = control_mode.to_sym

  case mode
  when :velocity
    set_velocity_control(joint, constraint, Float(target_velocity), Float(force))
  when :position
    set_position_control(joint, constraint, Float(target_position || 0.0), Float(force))
  when :torque
    set_torque_control(joint, Float(force))
  else
    raise ArgumentError, "unsupported joint control mode: #{control_mode}"
  end

  nil
end

#step_simulation(time_step: 1.0 / 240.0, max_sub_steps: 1, fixed_time_step: 1.0 / 240.0) ⇒ Object 



32
33
34
 
# File 'lib/bullet3/simulation.rb', line 32

def step_simulation(time_step: 1.0 / 240.0, max_sub_steps: 1, fixed_time_step: 1.0 / 240.0)
  world.step_simulation(time_step, max_sub_steps, fixed_time_step)
end