Skip to content

Commit

Permalink
Merge pull request #218 from NOSALRO/issue_211
Browse files Browse the repository at this point in the history 
Python cameras example fixes
  • Loading branch information
@costashatz
costashatz authored Aug 26, 2024
2 parents 4bf9753 + db6a1f0 commit 134e3a3
Show file tree
Hide file tree
Showing 5 changed files with 51 additions and 33 deletions.
17 changes: 9 additions & 8 deletions docs/faq/index.html
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1995,10 +1995,11 @@ <h2 id="how-can-i-attach-a-camera-to-a-moving-link"><strong>How can I attach a c
</div>
<div class="tabbed-block">
<div class="highlight"><pre><span></span><code><a id="__codelineno-23-1" name="__codelineno-23-1" href="#__codelineno-23-1"></a><span class="n">tf</span> <span class="o">=</span> <span class="n">dartpy</span><span class="o">.</span><span class="n">math</span><span class="o">.</span><span class="n">Isometry3</span><span class="p">()</span>
<a id="__codelineno-23-2" name="__codelineno-23-2" href="#__codelineno-23-2"></a><span class="n">rot</span> <span class="o">=</span> <span class="n">dartpy</span><span class="o">.</span><span class="n">math</span><span class="o">.</span><span class="n">AngleAxis</span><span class="p">(</span><span class="mf">3.14</span><span class="p">,</span> <span class="p">[</span><span class="mf">1.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">])</span>
<a id="__codelineno-23-3" name="__codelineno-23-3" href="#__codelineno-23-3"></a><span class="n">rot</span> <span class="o">=</span> <span class="n">rot</span><span class="o">.</span><span class="n">multiply</span><span class="p">(</span> <span class="n">dartpy</span><span class="o">.</span><span class="n">math</span><span class="o">.</span><span class="n">AngleAxis</span><span class="p">(</span><span class="mf">1.57</span><span class="p">,</span> <span class="p">[</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">]))</span><span class="o">.</span><span class="n">to_rotation_matrix</span><span class="p">()</span>
<a id="__codelineno-23-2" name="__codelineno-23-2" href="#__codelineno-23-2"></a><span class="n">rot</span> <span class="o">=</span> <span class="n">dartpy</span><span class="o">.</span><span class="n">math</span><span class="o">.</span><span class="n">AngleAxis</span><span class="p">(</span><span class="mf">3.14</span><span class="p">,</span> <span class="p">[</span><span class="mf">1.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">])</span>
<a id="__codelineno-23-3" name="__codelineno-23-3" href="#__codelineno-23-3"></a><span class="n">rot</span> <span class="o">=</span> <span class="n">rot</span><span class="o">.</span><span class="n">multiply</span><span class="p">(</span><span class="n">dartpy</span><span class="o">.</span><span class="n">math</span><span class="o">.</span><span class="n">AngleAxis</span><span class="p">(</span><span class="mf">1.57</span><span class="p">,</span> <span class="p">[</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">1.</span><span class="p">]))</span><span class="o">.</span><span class="n">to_rotation_matrix</span><span class="p">()</span>
<a id="__codelineno-23-4" name="__codelineno-23-4" href="#__codelineno-23-4"></a><span class="n">tf</span><span class="o">.</span><span class="n">set_translation</span><span class="p">([</span><span class="mf">0.</span><span class="p">,</span> <span class="mf">0.</span><span class="p">,</span> <span class="mf">0.1</span><span class="p">])</span>
<a id="__codelineno-23-5" name="__codelineno-23-5" href="#__codelineno-23-5"></a><span class="n">camera</span><span class="o">.</span><span class="n">attach_to_body</span><span class="p">(</span><span class="n">robot</span><span class="o">.</span><span class="n">body_node</span><span class="p">(</span><span class="s2">&quot;iiwa_link_ee&quot;</span><span class="p">),</span> <span class="n">tf</span><span class="p">)</span> <span class="c1"># cameras are looking towards -z by default</span>
<a id="__codelineno-23-5" name="__codelineno-23-5" href="#__codelineno-23-5"></a><span class="n">tf</span><span class="o">.</span><span class="n">set_rotation</span><span class="p">(</span><span class="n">rot</span><span class="p">)</span>
<a id="__codelineno-23-6" name="__codelineno-23-6" href="#__codelineno-23-6"></a><span class="n">camera</span><span class="o">.</span><span class="n">attach_to_body</span><span class="p">(</span><span class="n">robot</span><span class="o">.</span><span class="n">body_node</span><span class="p">(</span><span class="s2">&quot;iiwa_link_ee&quot;</span><span class="p">),</span> <span class="n">tf</span><span class="p">)</span> <span class="c1"># cameras are looking towards -z by default</span>
</code></pre></div>
</div>
</div>
Expand Down Expand Up @@ -2034,11 +2035,11 @@ <h2 id="how-can-i-manipulate-the-camera-object"><strong>How can I manipulate the
</code></pre></div>
</div>
<div class="tabbed-block">
<div class="highlight"><pre><span></span><code><a id="__codelineno-27-1" name="__codelineno-27-1" href="#__codelineno-27-1"></a><span class="n">camera</span><span class="o">.</span><span class="n">camera</span><span class="p">()</span><span class="o">.</span><span class="n">set_camera_params</span><span class="p">(</span><span class="mf">5.</span><span class="p">,</span> <span class="c1">#far plane</span>
<a id="__codelineno-27-2" name="__codelineno-27-2" href="#__codelineno-27-2"></a> <span class="mf">0.01</span><span class="p">,</span> <span class="c1">#near plane</span>
<a id="__codelineno-27-3" name="__codelineno-27-3" href="#__codelineno-27-3"></a> <span class="mf">60.0</span><span class="p">,</span> <span class="c1"># field of view</span>
<a id="__codelineno-27-4" name="__codelineno-27-4" href="#__codelineno-27-4"></a> <span class="mi">600</span><span class="p">,</span> <span class="c1"># width</span>
<a id="__codelineno-27-5" name="__codelineno-27-5" href="#__codelineno-27-5"></a> <span class="mi">400</span><span class="p">)</span> <span class="c1">#height</span>
<div class="highlight"><pre><span></span><code><a id="__codelineno-27-1" name="__codelineno-27-1" href="#__codelineno-27-1"></a><span class="n">camera</span><span class="o">.</span><span class="n">camera</span><span class="p">()</span><span class="o">.</span><span class="n">set_camera_params</span><span class="p">(</span><span class="mf">5.</span><span class="p">,</span> <span class="c1"># far plane</span>
<a id="__codelineno-27-2" name="__codelineno-27-2" href="#__codelineno-27-2"></a> <span class="mf">0.01</span><span class="p">,</span> <span class="c1"># near plane</span>
<a id="__codelineno-27-3" name="__codelineno-27-3" href="#__codelineno-27-3"></a> <span class="mf">60.0</span><span class="p">,</span> <span class="c1"># field of view</span>
<a id="__codelineno-27-4" name="__codelineno-27-4" href="#__codelineno-27-4"></a> <span class="mi">600</span><span class="p">,</span> <span class="c1"># width</span>
<a id="__codelineno-27-5" name="__codelineno-27-5" href="#__codelineno-27-5"></a> <span class="mi">400</span><span class="p">)</span> <span class="c1"># height</span>
</code></pre></div>
</div>
</div>
Expand Down
2 changes: 1 addition & 1 deletion docs/search/search_index.json
View file

Large diffs are not rendered by default.

2 changes: 1 addition & 1 deletion src/docs/include/macros.py
View file

Large diffs are not rendered by default.

11 changes: 9 additions & 2 deletions src/examples/cameras.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,8 +92,15 @@ int main()
tf.translation() = Eigen::Vector3d(0., 0., 0.1);
camera->attach_to_body(robot->body_node("iiwa_link_ee"), tf); // cameras are looking towards -z by default
// @CAM_ATTACH_END@

simu.add_checkerboard_floor(10.);
// the default checkerboard floor values are the following:
// double floor_width = 10.0
// double floor_height = 0.1
// double size = 1.
// const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()
// const std::string& floor_name = "checkerboard_floor"
// const Eigen::Vector4d& first_color = dart::Color::White(1.)
// const Eigen::Vector4d& second_color = dart::Color::Gray(1.))
simu.add_checkerboard_floor();
simu.add_robot(robot);
Eigen::Vector6d pose;
pose << 0., 0., 0., 1.5, 0., 0.25;
Expand Down
52 changes: 31 additions & 21 deletions src/examples/python/cameras.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
import dartpy
robot = rd.Iiwa()

ctrl = [-0.3, np.pi/3, 0.2, -np.pi, 4., 0., 0., -0.6]
ctrl = [-0.3, np.pi / 3., 0.2, -np.pi / 4., 0., 0., -0.6]
controller = rd.PDControl(ctrl)
robot.add_controller(controller)
controller.set_pd(500., 50.)
Expand Down Expand Up @@ -37,14 +37,14 @@
# @MANIPULATE_CAM_PYTHON_SEP_END@

# @MANIPULATE_CAM_PYTHON@
camera.camera().set_camera_params(5., #far plane
0.01, #near plane
60.0, # field of view
600, # width
400) #height
camera.camera().set_camera_params(5., # far plane
0.01, # near plane
60.0, # field of view
600, # width
400) # height
# @MANIPULATE_CAM_PYTHON_END@

camera.camera().record(True, True) # cameras are recording color images by default, enable depth images as well for this example
camera.camera().record(True, True) # cameras are recording color images by default, enable depth images as well for this example

# @RECORD_VIDEO_CAMERA_PYTHON@

Expand All @@ -61,19 +61,27 @@

# @CAM_ATTACH_PYTHON@
tf = dartpy.math.Isometry3()
rot = dartpy.math.AngleAxis(3.14, [1., 0., 0.])
rot = rot.multiply( dartpy.math.AngleAxis(1.57, [0., 0., 1.])).to_rotation_matrix()
rot = dartpy.math.AngleAxis(3.14, [1., 0., 0.])
rot = rot.multiply(dartpy.math.AngleAxis(1.57, [0., 0., 1.])).to_rotation_matrix()
tf.set_translation([0., 0., 0.1])
camera.attach_to_body(robot.body_node("iiwa_link_ee"), tf) # cameras are looking towards -z by default
tf.set_rotation(rot)
camera.attach_to_body(robot.body_node("iiwa_link_ee"), tf) # cameras are looking towards -z by default
# @CAM_ATTACH_PYTHON_END@

simu.add_checkerboard_floor(10.)
# the default checkerboard floor values are the following:
# floor_width = 10.0
# floor_height = 0.1,
# size = 1.,
# tf = dartpy.math.Isometry3.Identity() (transformation matrix of the floor)
# floor_name = "checkerboard_floor",
# first_color = magnum.Color3(0, 0, 0),
# second_color = magnum.Color3(128, 128, 128)
simu.add_checkerboard_floor()
simu.add_robot(robot)


pose = [ 0., 0., 0., 1.5, 0., 0.25]
pose = [0., 0., 0., 1.5, 0., 0.25]
simu.add_robot(rd.Robot.create_box([0.1, 0.1, 0.5], pose, "free", 1., [1, 0, 0, 1], "box"))
pose = [ 0., 0., 0., 1.5, -0.5, 0.25]
pose = [0., 0., 0., 1.5, -0.5, 0.25]
simu.add_robot(rd.Robot.create_ellipsoid([0.2, 0.2, 0.2], pose, "free", 1., [1, 0, 0, 1], "sphere"))
simu.add_sensor(camera)

Expand All @@ -99,12 +107,13 @@

depth_image = camera.depth_array()

small_box = rd.Robot.create_box([0.01, 0.01, 0.01], np.zeros(6), "fixed", 1., [0,0,1,1], "box_pc")
point_cloud = rd.gui.point_cloud_from_depth_array(depth_image, camera.camera_intrinsic_matrix(), camera.camera_extrinsic_matrix(), camera.camera().far_plane())
small_box = rd.Robot.create_box([0.01, 0.01, 0.01], np.zeros(6), "fixed", 1., [0, 0, 1, 1], "box_pc")
point_cloud = rd.gui.point_cloud_from_depth_array(
depth_image, camera.camera_intrinsic_matrix(), camera.camera_extrinsic_matrix(), camera.camera().far_plane())
for i in range(len(point_cloud)):
if (i % 30 == 0): # do not display all the points in the cloud
pose = 0., 0., 0., point_cloud[i]
bbox = small_box.clone_ghost("box_" + str(i), [0, 0, 1,1])
if (i % 30 == 0): # do not display all the points in the cloud
pose = 0., 0., 0., point_cloud[i]
bbox = small_box.clone_ghost("box_" + str(i), [0, 0, 1, 1])
bbox.set_base_pose(pose)
bbox.set_cast_shadows(False)
simu.add_robot(bbox)
Expand All @@ -118,8 +127,9 @@
break
if (simu.schedule(simu.graphics_freq())):
depth_image = camera.depth_array()
point_cloud = rd.gui.point_cloud_from_depth_array(depth_image, camera.camera_intrinsic_matrix(), camera.camera_extrinsic_matrix(), camera.camera().far_plane())
print( simu.scheduler().current_time(), ": ", len(point_cloud))
point_cloud = rd.gui.point_cloud_from_depth_array(
depth_image, camera.camera_intrinsic_matrix(), camera.camera_extrinsic_matrix(), camera.camera().far_plane())
print(simu.scheduler().current_time(), ": ", len(point_cloud))


robot.reset()

0 comments on commit 134e3a3

Please sign in to comment.