Skip to content

Commit c418693

Browse files
gonzalocasasgonzalocasas
committed
add attached/non-attached collision mesh viz support to GH component
1 parent a6d23da commit c418693

File tree

4 files changed

+92
-27
lines changed

4 files changed

+92
-27
lines changed

‎CHANGELOG.rst‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,12 @@ Unreleased
1212

1313
**Added**
1414

15+
* Added support for attached and non-attached collision mesh visualization to the ``Robot Visualize`` GH component.
16+
1517
**Changed**
1618

19+
* Changed behavior of ``Attach Tool`` GH component to only attach the tool but not add it to the planning scene state.
20+
1721
**Fixed**
1822

1923
* Fixed DH params for analytical IK solver of UR3e and UR10e.

‎src/compas_fab/ghpython/components/Cf_AttachTool/code.py‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,6 @@
88
from compas_rhino.conversions import plane_to_compas_frame
99

1010
from compas.geometry import Frame
11-
from compas_fab.robots import PlanningScene
1211
from compas_fab.robots import Tool
1312

1413

@@ -27,8 +26,6 @@ def RunScript(self, robot, visual_mesh, collision_mesh, tcf_plane, group):
2726
frame = plane_to_compas_frame(tcf_plane)
2827
tool = Tool(c_visual_mesh, frame, c_collision_mesh)
2928

30-
scene = PlanningScene(robot)
3129
robot.attach_tool(tool, group)
32-
scene.add_attached_tool()
3330

3431
return robot

‎src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py‎

Lines changed: 76 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -3,25 +3,40 @@
33
44
COMPAS FAB v0.26.0
55
"""
6+
from compas.artists import Artist
7+
from compas.geometry import Frame
68
from compas.geometry import Transformation
7-
from compas_ghpython.artists import FrameArtist
8-
from compas_ghpython.artists import MeshArtist
9+
from compas_fab.robots import PlanningScene
910
from ghpythonlib.componentbase import executingcomponent as component
1011

1112

1213
class RobotVisualize(component):
13-
def RunScript(self, robot, group, configuration, attached_collision_meshes, show_visual, show_collision,
14-
show_frames, show_base_frame, show_end_effector_frame, show_acm):
14+
def RunScript(
15+
self,
16+
robot,
17+
group,
18+
configuration,
19+
show_visual,
20+
show_collision,
21+
show_frames,
22+
show_base_frame,
23+
show_end_effector_frame,
24+
show_cm,
25+
show_acm,
26+
):
1527

1628
visual = None
1729
collision = None
30+
collision_meshes = None
1831
attached_meshes = None
1932
frames = None
2033
base_frame = None
2134
ee_frame = None
2235

2336
if robot:
2437
show_visual = True if show_visual is None else show_visual
38+
show_cm = True if show_cm is None else show_cm
39+
show_acm = True if show_acm is None else show_acm
2540
configuration = configuration or robot.zero_configuration()
2641

2742
robot.update(configuration, visual=show_visual, collision=show_collision)
@@ -35,27 +50,73 @@ def RunScript(self, robot, group, configuration, attached_collision_meshes, show
3550

3651
if show_base_frame:
3752
base_compas_frame = compas_frames[0]
38-
artist = FrameArtist(base_compas_frame)
53+
artist = Artist(base_compas_frame)
3954
base_frame = artist.draw()
4055

4156
if show_end_effector_frame:
42-
ee_compas_frame = robot.forward_kinematics(configuration, group, options=dict(solver='model'))
43-
artist = FrameArtist(ee_compas_frame)
57+
ee_compas_frame = robot.forward_kinematics(
58+
configuration, group, options=dict(solver="model")
59+
)
60+
artist = Artist(ee_compas_frame)
4461
ee_frame = artist.draw()
4562

4663
if show_frames:
4764
frames = []
4865
for compas_frame in compas_frames[1:]:
49-
artist = FrameArtist(compas_frame)
66+
artist = Artist(compas_frame)
5067
frame = artist.draw()
5168
frames.append(frame)
5269

53-
if show_acm:
70+
if show_cm or show_acm:
71+
scene = PlanningScene(robot)
72+
scene = robot.client.get_planning_scene()
73+
74+
collision_meshes = []
5475
attached_meshes = []
55-
for acm in attached_collision_meshes:
56-
frame = robot.forward_kinematics(configuration, options=dict(solver='model', link=acm.link_name))
57-
T = Transformation.from_frame_to_frame(acm.collision_mesh.frame, frame)
58-
mesh = acm.collision_mesh.mesh.transformed(T)
59-
attached_meshes.append(MeshArtist(mesh).draw())
6076

61-
return (visual, collision, attached_meshes, frames, base_frame, ee_frame)
77+
if show_cm:
78+
for co in scene.world.collision_objects:
79+
header = co.header
80+
frame_id = header.frame_id
81+
cms = co.to_collision_meshes()
82+
83+
for cm in cms:
84+
if cm.frame != Frame.worldXY():
85+
t = Transformation.from_frame(cm.frame)
86+
mesh = cm.mesh.transformed(t)
87+
else:
88+
mesh = cm.mesh
89+
90+
collision_meshes.append(Artist(mesh).draw())
91+
92+
if show_acm:
93+
for aco in scene.robot_state.attached_collision_objects:
94+
for acm in aco.to_attached_collision_meshes():
95+
frame_id = aco.object.header.frame_id
96+
frame = robot.forward_kinematics(
97+
configuration, options=dict(link=frame_id)
98+
)
99+
t = Transformation.from_frame(frame)
100+
101+
# Local CM frame
102+
if (
103+
acm.collision_mesh.frame
104+
and acm.collision_mesh.frame != Frame.worldXY()
105+
):
106+
t = t * Transformation.from_frame(
107+
acm.collision_mesh.frame
108+
)
109+
110+
mesh = acm.collision_mesh.mesh.transformed(t)
111+
112+
attached_meshes.append(Artist(mesh).draw())
113+
114+
return (
115+
visual,
116+
collision,
117+
collision_meshes,
118+
attached_meshes,
119+
frames,
120+
base_frame,
121+
ee_frame,
122+
)

‎src/compas_fab/ghpython/components/Cf_VisualizeRobot/metadata.json‎

Lines changed: 12 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@
55
"subcategory": "Display",
66
"description": "Visualizes the robot.",
77
"exposure": 2,
8-
98
"ghpython": {
109
"isAdvancedMode": true,
1110
"iconDisplay": 2,
@@ -24,11 +23,6 @@
2423
"name": "configuration",
2524
"description": "The robot's full configuration."
2625
},
27-
{
28-
"name": "attached_collision_meshes",
29-
"description": "A list of attached collision meshes.",
30-
"scriptParamAccess": "list"
31-
},
3226
{
3327
"name": "show_visual",
3428
"description": "Whether or not to show the robot's visual meshes.",
@@ -54,6 +48,11 @@
5448
"description": "Whether or not to show the robot's end-effector frame.",
5549
"typeHintID": "bool"
5650
},
51+
{
52+
"name": "show_cm",
53+
"description": "Whether or not to show the collision meshes (if any).",
54+
"typeHintID": "bool"
55+
},
5756
{
5857
"name": "show_acm",
5958
"description": "Whether or not to show the attached collision meshes (if any).",
@@ -63,15 +62,19 @@
6362
"outputParameters": [
6463
{
6564
"name": "visual",
66-
"description": "The robot's visual Rhino meshes (if any)."
65+
"description": "Rhino meshes of the robot's visual geometry (if any)."
6766
},
6867
{
6968
"name": "collision",
70-
"description": "The robot's collision Rhino meshes (if any)."
69+
"description": "Rhino meshes of the robot's collision geometry (if any)."
70+
},
71+
{
72+
"name": "collision_meshes",
73+
"description": "Rhino meshes of the scene's collision meshes (if any)."
7174
},
7275
{
7376
"name": "attached_meshes",
74-
"description": "The attached collision Rhino meshes (if any)."
77+
"description": "Rhino meshes of the scene's attached collision meshes (if any)."
7578
},
7679
{
7780
"name": "frames",

0 commit comments

Comments
 (0)