|
3 | 3 | import dartpy |
4 | 4 | robot = rd.Iiwa() |
5 | 5 |
|
6 | | -ctrl = [-0.3, np.pi/3, 0.2, -np.pi, 4., 0., 0., -0.6] |
| 6 | +ctrl = [-0.3, np.pi / 3., 0.2, -np.pi / 4., 0., 0., -0.6] |
7 | 7 | controller = rd.PDControl(ctrl) |
8 | 8 | robot.add_controller(controller) |
9 | 9 | controller.set_pd(500., 50.) |
|
37 | 37 | # @MANIPULATE_CAM_PYTHON_SEP_END@ |
38 | 38 |
|
39 | 39 | # @MANIPULATE_CAM_PYTHON@ |
40 | | -camera.camera().set_camera_params(5., #far plane |
41 | | - 0.01, #near plane |
42 | | - 60.0, # field of view |
43 | | - 600, # width |
44 | | - 400) #height |
| 40 | +camera.camera().set_camera_params(5., # far plane |
| 41 | + 0.01, # near plane |
| 42 | + 60.0, # field of view |
| 43 | + 600, # width |
| 44 | + 400) # height |
45 | 45 | # @MANIPULATE_CAM_PYTHON_END@ |
46 | 46 |
|
47 | | -camera.camera().record(True, True) # cameras are recording color images by default, enable depth images as well for this example |
| 47 | +camera.camera().record(True, True) # cameras are recording color images by default, enable depth images as well for this example |
48 | 48 |
|
49 | 49 | # @RECORD_VIDEO_CAMERA_PYTHON@ |
50 | 50 |
|
|
61 | 61 |
|
62 | 62 | # @CAM_ATTACH_PYTHON@ |
63 | 63 | tf = dartpy.math.Isometry3() |
64 | | -rot = dartpy.math.AngleAxis(3.14, [1., 0., 0.]) |
65 | | -rot = rot.multiply( dartpy.math.AngleAxis(1.57, [0., 0., 1.])).to_rotation_matrix() |
| 64 | +rot = dartpy.math.AngleAxis(3.14, [1., 0., 0.]) |
| 65 | +rot = rot.multiply(dartpy.math.AngleAxis(1.57, [0., 0., 1.])).to_rotation_matrix() |
66 | 66 | tf.set_translation([0., 0., 0.1]) |
67 | | -camera.attach_to_body(robot.body_node("iiwa_link_ee"), tf) # cameras are looking towards -z by default |
| 67 | +tf.set_rotation(rot) |
| 68 | +camera.attach_to_body(robot.body_node("iiwa_link_ee"), tf) # cameras are looking towards -z by default |
68 | 69 | # @CAM_ATTACH_PYTHON_END@ |
69 | | - |
70 | | -simu.add_checkerboard_floor(10.) |
| 70 | +# the default checkerboard floor values are the following: |
| 71 | +# floor_width = 10.0 |
| 72 | +# floor_height = 0.1, |
| 73 | +# size = 1., |
| 74 | +# tf = dartpy.math.Isometry3.Identity() (transformation matrix of the floor) |
| 75 | +# floor_name = "checkerboard_floor", |
| 76 | +# first_color = magnum.Color3(0, 0, 0), |
| 77 | +# second_color = magnum.Color3(128, 128, 128) |
| 78 | +simu.add_checkerboard_floor() |
71 | 79 | simu.add_robot(robot) |
72 | 80 |
|
73 | 81 |
|
74 | | -pose = [ 0., 0., 0., 1.5, 0., 0.25] |
| 82 | +pose = [0., 0., 0., 1.5, 0., 0.25] |
75 | 83 | simu.add_robot(rd.Robot.create_box([0.1, 0.1, 0.5], pose, "free", 1., [1, 0, 0, 1], "box")) |
76 | | -pose = [ 0., 0., 0., 1.5, -0.5, 0.25] |
| 84 | +pose = [0., 0., 0., 1.5, -0.5, 0.25] |
77 | 85 | simu.add_robot(rd.Robot.create_ellipsoid([0.2, 0.2, 0.2], pose, "free", 1., [1, 0, 0, 1], "sphere")) |
78 | 86 | simu.add_sensor(camera) |
79 | 87 |
|
|
99 | 107 |
|
100 | 108 | depth_image = camera.depth_array() |
101 | 109 |
|
102 | | -small_box = rd.Robot.create_box([0.01, 0.01, 0.01], np.zeros(6), "fixed", 1., [0,0,1,1], "box_pc") |
103 | | -point_cloud = rd.gui.point_cloud_from_depth_array(depth_image, camera.camera_intrinsic_matrix(), camera.camera_extrinsic_matrix(), camera.camera().far_plane()) |
| 110 | +small_box = rd.Robot.create_box([0.01, 0.01, 0.01], np.zeros(6), "fixed", 1., [0, 0, 1, 1], "box_pc") |
| 111 | +point_cloud = rd.gui.point_cloud_from_depth_array( |
| 112 | + depth_image, camera.camera_intrinsic_matrix(), camera.camera_extrinsic_matrix(), camera.camera().far_plane()) |
104 | 113 | for i in range(len(point_cloud)): |
105 | | - if (i % 30 == 0): # do not display all the points in the cloud |
106 | | - pose = 0., 0., 0., point_cloud[i] |
107 | | - bbox = small_box.clone_ghost("box_" + str(i), [0, 0, 1,1]) |
| 114 | + if (i % 30 == 0): # do not display all the points in the cloud |
| 115 | + pose = 0., 0., 0., point_cloud[i] |
| 116 | + bbox = small_box.clone_ghost("box_" + str(i), [0, 0, 1, 1]) |
108 | 117 | bbox.set_base_pose(pose) |
109 | 118 | bbox.set_cast_shadows(False) |
110 | 119 | simu.add_robot(bbox) |
|
118 | 127 | break |
119 | 128 | if (simu.schedule(simu.graphics_freq())): |
120 | 129 | depth_image = camera.depth_array() |
121 | | - point_cloud = rd.gui.point_cloud_from_depth_array(depth_image, camera.camera_intrinsic_matrix(), camera.camera_extrinsic_matrix(), camera.camera().far_plane()) |
122 | | - print( simu.scheduler().current_time(), ": ", len(point_cloud)) |
| 130 | + point_cloud = rd.gui.point_cloud_from_depth_array( |
| 131 | + depth_image, camera.camera_intrinsic_matrix(), camera.camera_extrinsic_matrix(), camera.camera().far_plane()) |
| 132 | + print(simu.scheduler().current_time(), ": ", len(point_cloud)) |
123 | 133 |
|
124 | 134 |
|
125 | 135 | robot.reset() |
0 commit comments