From c496fd6ea9d5729a679e78b5640ff97366371115 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 14:31:57 +0100 Subject: [PATCH 01/36] updated to compas_invocations2 --- requirements-dev.txt | 2 +- tasks.py | 16 ++++++++++++---- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/requirements-dev.txt b/requirements-dev.txt index b3619e19db..ca0d48fc35 100644 --- a/requirements-dev.txt +++ b/requirements-dev.txt @@ -3,7 +3,7 @@ autopep8 black bump2version >=1.0.1 check-manifest >=0.36 -compas_invocations +compas_invocations2 doc8 flake8 importlib_metadata <5.0 diff --git a/tasks.py b/tasks.py index c75addbba2..f5c1280a23 100644 --- a/tasks.py +++ b/tasks.py @@ -2,10 +2,11 @@ import os -from compas_invocations import build -from compas_invocations import docs -from compas_invocations import style -from compas_invocations import tests +from compas_invocations2 import build +from compas_invocations2 import docs +from compas_invocations2 import style +from compas_invocations2 import tests +from compas_invocations2 import grasshopper from invoke import Collection ns = Collection( @@ -22,6 +23,8 @@ build.clean, build.release, build.build_ghuser_components, + build.build_cpython_ghuser_components, + grasshopper.yakerize, ) ns.configure( { @@ -31,5 +34,10 @@ "target_dir": "src/compas_fab/ghpython/components/ghuser", "prefix": "COMPAS FAB: ", }, + "ghuser_cpython": { + "source_dir": "src/compas_fab/ghpython/components_cpython", + "target_dir": "src/compas_fab/ghpython/components_cpython/ghuser", + "prefix": "COMPAS FAB: ", + }, } ) From 5564dd388fc63c77b5438b8ce960e82bb58ba896 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 14:32:34 +0100 Subject: [PATCH 02/36] created cpython component definitions --- .gitignore | 1 + .../components_cpython/Cf_AttachTool/code.py | 34 ++++ .../components_cpython/Cf_AttachTool/icon.png | Bin 0 -> 1122 bytes .../Cf_AttachTool/metadata.json | 48 ++++++ .../Cf_AttachedCollisionMesh/code.py | 28 ++++ .../Cf_AttachedCollisionMesh/icon.png | Bin 0 -> 1743 bytes .../Cf_AttachedCollisionMesh/metadata.json | 57 +++++++ .../Cf_CollisionMesh/code.py | 39 +++++ .../Cf_CollisionMesh/icon.png | Bin 0 -> 1708 bytes .../Cf_CollisionMesh/metadata.json | 50 ++++++ .../components_cpython/Cf_ConfigMerge/code.py | 16 ++ .../Cf_ConfigMerge/icon.png | Bin 0 -> 5579 bytes .../Cf_ConfigMerge/metadata.json | 29 ++++ .../components_cpython/Cf_ConfigZero/code.py | 16 ++ .../components_cpython/Cf_ConfigZero/icon.png | Bin 0 -> 2074 bytes .../Cf_ConfigZero/metadata.json | 31 ++++ .../Cf_ConfigurationTarget/code.py | 32 ++++ .../Cf_ConfigurationTarget/icon.png | Bin 0 -> 680 bytes .../Cf_ConfigurationTarget/metadata.json | 39 +++++ .../Cf_ForwardKinematics/code.py | 21 +++ .../Cf_ForwardKinematics/icon.png | Bin 0 -> 2709 bytes .../Cf_ForwardKinematics/metadata.json | 44 +++++ .../Cf_FrameTargetFromPlane/code.py | 44 +++++ .../Cf_FrameTargetFromPlane/icon.png | Bin 0 -> 620 bytes .../Cf_FrameTargetFromPlane/metadata.json | 48 ++++++ .../Cf_InverseKinematics/code.py | 19 +++ .../Cf_InverseKinematics/icon.png | Bin 0 -> 2734 bytes .../Cf_InverseKinematics/metadata.json | 39 +++++ .../Cf_PlanCartesianMotion/code.py | 51 ++++++ .../Cf_PlanCartesianMotion/icon.png | Bin 0 -> 1594 bytes .../Cf_PlanCartesianMotion/metadata.json | 60 +++++++ .../components_cpython/Cf_PlanMotion/code.py | 47 ++++++ .../components_cpython/Cf_PlanMotion/icon.png | Bin 0 -> 1701 bytes .../Cf_PlanMotion/metadata.json | 60 +++++++ .../Cf_PlanningScene/code.py | 21 +++ .../Cf_PlanningScene/icon.png | Bin 0 -> 1706 bytes .../Cf_PlanningScene/metadata.json | 26 +++ .../Cf_PointAxisTarget/code.py | 28 ++++ .../Cf_PointAxisTarget/icon.png | Bin 0 -> 620 bytes .../Cf_PointAxisTarget/metadata.json | 37 +++++ .../components_cpython/Cf_RosConnect/code.py | 34 ++++ .../components_cpython/Cf_RosConnect/icon.png | Bin 0 -> 1805 bytes .../Cf_RosConnect/metadata.json | 40 +++++ .../components_cpython/Cf_RosRobot/code.py | 28 ++++ .../components_cpython/Cf_RosRobot/icon.png | Bin 0 -> 1668 bytes .../Cf_RosRobot/metadata.json | 30 ++++ .../Cf_RosTopicPublish/code.py | 48 ++++++ .../Cf_RosTopicPublish/icon.png | Bin 0 -> 1081 bytes .../Cf_RosTopicPublish/metadata.json | 42 +++++ .../Cf_RosTopicSubscribe/code.py | 82 +++++++++ .../Cf_RosTopicSubscribe/icon.png | Bin 0 -> 1098 bytes .../Cf_RosTopicSubscribe/metadata.json | 57 +++++++ .../Cf_VisualizeRobot/code.py | 156 ++++++++++++++++++ .../Cf_VisualizeRobot/icon.png | Bin 0 -> 1781 bytes .../Cf_VisualizeRobot/metadata.json | 93 +++++++++++ .../Cf_VisualizeTrajectory/code.py | 46 ++++++ .../Cf_VisualizeTrajectory/icon.png | Bin 0 -> 1798 bytes .../Cf_VisualizeTrajectory/metadata.json | 63 +++++++ 58 files changed, 1684 insertions(+) create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_AttachTool/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_AttachTool/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_FrameTargetFromPlane/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_FrameTargetFromPlane/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_FrameTargetFromPlane/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PointAxisTarget/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PointAxisTarget/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PointAxisTarget/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_RosConnect/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_RosConnect/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_RosRobot/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_RosRobot/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/metadata.json diff --git a/.gitignore b/.gitignore index 71c538a6e8..f934e9b26d 100644 --- a/.gitignore +++ b/.gitignore @@ -120,3 +120,4 @@ temp/** # Grasshopper generated objects src/compas_fab/ghpython/components/ghuser/*.ghuser +src/compas_fab/ghpython/components_cpython/ghuser/*.ghuser diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py new file mode 100644 index 0000000000..61568d71c3 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py @@ -0,0 +1,34 @@ +# r: compas_fab>=1.0.2 +""" +Attach a tool to the robot. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + +from compas.geometry import Frame +from compas_rhino.conversions import mesh_to_compas +from compas_rhino.conversions import plane_to_compas_frame + +from compas_fab.robots import Tool + + +class AttachToolComponent(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, robot, visual_mesh, collision_mesh, tcf_plane, group, connected_to): + if robot and robot.client and robot.client.is_connected and visual_mesh: + if not collision_mesh: + collision_mesh = visual_mesh + + c_visual_mesh = mesh_to_compas(visual_mesh) + c_collision_mesh = mesh_to_compas(collision_mesh) + + if not tcf_plane: + frame = Frame.worldXY() + else: + frame = plane_to_compas_frame(tcf_plane) + tool = Tool(c_visual_mesh, frame, c_collision_mesh, connected_to=connected_to) + + robot.attach_tool(tool, group) + + return robot diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..50e13aa3fec3754b2012c7d37dfc4a76d5559b39 GIT binary patch literal 1122 zcmV-o1fBbdP)SmBAGKnNGZt6zs>YxgiwUnNo%ln#Bg`nUl(SP!MI62Sr{NCsC zCNCIcE1Gs?oLqjoQOLyipT;2rMcPk|*(CdP_~jG!O#By%Sd<$n9B3i{=3_oB+ zavb-RnN+(sbbgQ#Ad?CN06HN8pqu5)q!$F2^Xjm__J6AY;K)~ArLqbHgfL*_;u}mV zzWIvHuveDpXF5DH@fNyEC^8zZZvrI zOwYMjrgV;b=%%8=_SnGSjO@wgif$`=f>%?=CO_&3th$25au(#*dMYcNS}>Gkf?(tF za&_WRdemsN)Zy_6@&18ri64HFm9o<0eAtRZUaSk{o6}!qdv?kIOjFa2UF-es$u;Xr zkO*JnV<{Ehd+t6zJ36*)evMZJNN*H?*rg%2YeV_*@h`geRqcKbM6m;^j^1-GAuB&m zqnhRhkg_0|ZP8z{W?>nCL#c_$dXIN4I7tL)d>F(of)hn+rDFgVZv3idU}@N2+X&z> zno6038a$FZ!QoHAbP8Ef45sNOfQ1=k3MglW@yNuBuRRJva(GdcCA+Li4%zWrqzAu* zLon0?WZ6a;WqttKd|;+6s?l3Jnu_AZ>&q+ZJy}^^8!-&W+7drjd(i zJ#g=AV($CJftdnqqUGAO%i0SVYMdvGfe-@j@%tf3GRgwa>9A}$EdkKJ{u+T7Kmt2J znF*5`(3uIm`Xy4)n3bHleO`l6>azU%-)lo+ZUEWMb;|<&b`PdsKh=;7hVC@;4Dd%f z(Njq58BoJ*7V%6Qvd(}07*qoM6N<$f;+Jr-T(jq literal 0 HcmV?d00001 diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/metadata.json new file mode 100644 index 0000000000..d4aafacbae --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/metadata.json @@ -0,0 +1,48 @@ +{ + "name": "Attached Tool", + "nickname": "Tool", + "category": "COMPAS FAB", + "subcategory": "Scene", + "description": "Attach a tool to the robot.", + "exposure": 4, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "visual_mesh", + "description": "A visual mesh for the tool.", + "typeHintID": "mesh" + }, + { + "name": "collision_mesh", + "description": "A collision mesh for the tool.", + "typeHintID": "mesh" + }, + { + "name": "tcf_plane", + "description": "The frame of the tool in tool0 frame." + }, + { + "name": "group", + "description": "The planning group to which the tool should be attached." + }, + { + "name": "connected_to", + "description": "The link to which the tool should be connected to." + } + ], + "outputParameters": [ + { + "name": "robot", + "description": "The robot." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py new file mode 100644 index 0000000000..c2144a4555 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py @@ -0,0 +1,28 @@ +# r: compas_fab>=1.0.2 +""" +Add an attached collision mesh to the robot. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + +from compas_rhino.conversions import mesh_to_compas + +from compas_fab.robots import AttachedCollisionMesh +from compas_fab.robots import CollisionMesh + + +class AttachedCollisionMeshComponent(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, scene, mesh, identifier, link_name, touch_links, add, remove): + attached_collision_mesh = None + if scene and mesh and identifier and link_name: + compas_mesh = mesh_to_compas(mesh) + collision_mesh = CollisionMesh(compas_mesh, identifier) + attached_collision_mesh = AttachedCollisionMesh(collision_mesh, link_name, touch_links) + if add: + scene.add_attached_collision_mesh(attached_collision_mesh) + if remove: + scene.remove_attached_collision_mesh(identifier) + scene.remove_collision_mesh(identifier) + return attached_collision_mesh diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..ce20f794b77f7bdff04bf6d52b180ce4869da853 GIT binary patch literal 1743 zcmbVNZA=?w9KV)P$AFDEfo;KX9$OH{^vbBi?WL>C>$$Gte+FodT$K62- zYKB{a6B4&X6JJ7HGW{_9K+G};Y(|Y}lthsa&bd!xCPv*1lR2|S$Le#XZJ0q9cgb_l zbIgzkJh1N)2~r4{Q*jxh5oAwWO6K@JprIiU5fcvV#)XeDR1_RougypLWEY5v zo?!(X81D4*!+ks>U~R4Fo)ilS;y~ljR6LeY*^~pz^0IKPZxa}rg=l>atX&s~27KM9 zOHu%8HL5rSeEh9GHzrg4bC)uDvOrSODWmuGMTl~+Vr6Qu;IGjbtmKyzSF z(^V7Va)CCX=G+7wMx;2IpvRhXhzN<>!MZvkPL_(ph^P@4>|^+nsvH0GV4+Rr%8%m zl4AKlb>~$^Y06BakN1iRK}xC(Yc2pcrvV2BqekH*jgvM%MY9$=OEbGkiY3Was815a z@X)%@W|pS!fx?s#IE}k6Sm4>Pq{KOBuo&kefRGar3@x~kbxAQvfr6oSmbLp{m#bTm z!eR^_s6G4lqTUXdonq{EoHkQgb$ve8n@}|_!2_?`fkEe(MNwdByM+ncEIe)nHXf&% ztu)RsHVUUg_GUXH2u)$en!oRs_yIiy^7n=RdA~~$VF_}v2j+A*#~%n-Zd-g{53JHAqX3frM_#PLR4J?_IR&&wpttUucLY?P z&=ob|3*eoB~_fsrgwSf*1~a-R*u)IiJpC+KO;$ z_Du1yOU5@osxvw#kk87UHO|V?5mN)wZ>Xq6e#k`AMHK{kk$z-;Y&Y@K!KInsuY~4* z$pjkHH>T2eGTS3>-8r0GIoLe*SXr6T`5|)r(cca&&fNOc`1QMAWU7~@|2T5_$Lh@W z+gJXoJ$vnDX0E=ENv)!pOHUro==X$EuY&g}zhtv7a|myVh+yxBQfY+AVG+gM-r%@bRZ-ip_kZeMuqkp9c@c67QY_Xdyt4I%?T A761SM literal 0 HcmV?d00001 diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/metadata.json new file mode 100644 index 0000000000..408c71dc91 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/metadata.json @@ -0,0 +1,57 @@ +{ + "name": "Attached Collision Mesh", + "nickname": "ACM", + "category": "COMPAS FAB", + "subcategory": "Scene", + "description": "Add or remove an attached collision mesh to the robot.", + "exposure": 4, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "scene", + "description": "The planning scene." + }, + { + "name": "mesh", + "description": "A collision mesh.", + "typeHintID": "mesh" + }, + { + "name": "identifier", + "description": "The identifier of the collision mesh.", + "typeHintID": "str" + }, + { + "name": "link_name", + "description": "The robot's link name to attach the mesh to.", + "typeHintID": "str" + }, + { + "name": "touch_links", + "description": "Names of links that the robot is allowed to touch.", + "typeHintID": "str", + "scriptParamAccess": "list" + }, + { + "name": "add", + "description": "If True, adds the collision mesh to the planning scene.", + "typeHintID": "bool" + }, + { + "name": "remove", + "description": "If True, removes the collision mesh from the planning scene.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "attached_collision_mesh", + "description": "A collision mesh that is attached to a robot's link.", + "optional": false + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py new file mode 100644 index 0000000000..bc4f0fb75d --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py @@ -0,0 +1,39 @@ +# r: compas_fab>=1.0.2 +""" +Add or remove a collision mesh from the planning scene. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + +from compas_rhino.conversions import mesh_to_compas + +from compas_fab.robots import CollisionMesh + + +class CollisionMeshComponent(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, scene, M, name, add, append, remove): + ok = False + self.Message = "" + + if (add and append) or (append and remove) or (add and remove): + self.Message = "Use only one operation at a time\n(add, append or remove)" + raise Exception(self.Message) + + if scene and M and name: + mesh = mesh_to_compas(M) + collision_mesh = CollisionMesh(mesh, name) + if add: + scene.add_collision_mesh(collision_mesh) + self.Message = "Added" + ok = True + if append: + scene.append_collision_mesh(collision_mesh) + self.Message = "Appended" + ok = True + if remove: + scene.remove_collision_mesh(name) + self.Message = "Removed" + ok = True + return ok diff --git a/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..8fe77327663017a1074589c9cca3ed7956511c27 GIT binary patch literal 1708 zcmbVNe@xs|953?24TWJaX7i8PltoeL^#`TfHB?}C2X|w=owv)tWG3|5$6d$u2W{CM z7zBrtG5?q|ZYrRQge(ij`3o`8#EI@l!i;9RWE!)`788|>Eoh9yY4r8DyD)<;uF30r zeed%=-{0R3g@R2b#rGB?2vQR8d&6*E>x@-J@I9O47vZ$p@IPiD$Q|X*$U}xlA)@de zCDLlQ3R^izS6!m4M}aG)8W4>jwRI^&lsbToMnO!`cc2|SeloEYiFCl&@{yE;IRfrAlfQ~P>*f` zly#A~M3OYR(e0vGhGEGnlqPACAZrMU#wnH~8IIbBW*!VmGv#(J>}|}*f?qrqw{3$X zh-5P9N>;mcGe%Hux0@hof~IkZz^!i07E`!pRb&~wz>-YGuoYcH9Y!&#ciB7!HC-}6 zHFC6?m2nev7?BbUf^w0LNok-g<#0xqnMezlB?2UX3N+h-SSp7#;<~L{as6MYbH_Ib zfUXsUoQxH9QPrFX%l7StW@H?)BHD^{8-NG{OYbrz;M)z=tZ=L`IFAWLTQ?)Rp2!9& zlvNp}DHn}awkVpcC#?sUT>xIu20R9%M&TrllQj{F=Binac0Wu~97!%g1zlF!yH|v6 z;%MpyC`=hyw8fi(Wr=IoO;v;jE2w0;XWz7uJgTr3KWmmd6bd}y}F7+Cmh7hX7dy7`KK>ia_?T;Y2=ib7!QlUGg}!6Vqk#^TET zm#3%ZukAe=UiS=jvhxpCnEkbPP4}ywvVuOE70Smd_e`nhO6TtS__seFxX|V9ub+5p zK>3Q1XZk)R3(j;t*@!Hxxin}${=>+`#O1+NhrznpiWi2%r|cug@2h2mC+dn{+TkDc zT{-vAS<@xHQLu<_@jd(eqci2#U(8?oUFmZhhGv>i($ QL^=PHfG_AhzPWAB-%S-lp8x;= literal 0 HcmV?d00001 diff --git a/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/metadata.json new file mode 100644 index 0000000000..cee428daae --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/metadata.json @@ -0,0 +1,50 @@ +{ + "name": "Collision Mesh", + "nickname": "CM", + "category": "COMPAS FAB", + "subcategory": "Scene", + "description": "Add or remove a collision mesh from the planning scene.", + "exposure": 4, + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "scene", + "description": "The planning scene." + }, + { + "name": "mesh", + "description": "A collision mesh.", + "typeHintID": "mesh" + }, + { + "name": "identifier", + "description": "The identifier of the collision mesh.", + "typeHintID": "str" + }, + { + "name": "add", + "description": "If True, adds the collision mesh to the planning scene.", + "typeHintID": "bool" + }, + { + "name": "append", + "description": "If True, appends the collision mesh to the planning scene.", + "typeHintID": "bool" + }, + { + "name": "remove", + "description": "If True, removes the collision mesh from the planning scene.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "ok", + "description": "True if the operation was successfully performed.", + "optional": false + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/code.py new file mode 100644 index 0000000000..cdcde7e6ed --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/code.py @@ -0,0 +1,16 @@ +# r: compas_fab>=1.0.2 +""" +Merge two configurations. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + + +class ConfigMerge(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, config_a, config_b): + if config_a and config_b: + return config_a.merged(config_b) + + return None diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_ConfigMerge/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..0bd8c533a031c30c84d59849c78cd10f6f683c5a GIT binary patch literal 5579 zcmV;+6*TIJP)2=(8#hEYntFIlu-YXT~*oK+?1AO zF*-WR*47pa3k$Ndvy*f>O|4eLFbrm9W_bPjHLI(u1OfqU+h%=z{fC0zfp2YX$!s>u z)YOz*SXdAjFJ9!uix()0f^FN3jEvxNxtN=qqqDP<(a}*1!=O^BY|PKk2jMq8P*s)f z?QQwt!v{{DJjvA5lwz7DE|&{k*HIJ&UDx^YYiHQj=UApwA&*!_GPN!uw8fAERnC|XwQLR=9hr>b$!KqWHn46oE zrBaEpu`!MvJBG*OVPaxpFM-?bCY?^pL?VG{nq0VW;ZMKcf4NjD$@};3Gdw&@Pfw3% zG#WULL#|PRgcFbhlYlD{P;1ZY0}-@EgFpmE|-gXy-p^RLDzNa z^*Ya%`$w`iL7qa>rMfvp^OE7AD)IF94+ zPb**9mF+kVnx>J<zq(DvE+-S%g9%B9RDx zAK$-*3Q#N->FMb~*LAkHw<#8jbaZr(&*zz)oki0$3WWl}V347qAqEEru`G*@_70|} zr*RyI#l^*6_Cx{L*x0~v9LnV~<#M^@#M`%Ti;Wmz0JaDZSih?J60DD=Nuw{G39*X!uIj@RqOaUA;l z``Nc|A5uzGRYliz{C{rw0ba2zN5?%g~3`uYe20u+nIpE3#t zgKTYWwa#w0o0XLnjvqg6oH=twE-x>W%jK|b8vya2|5f}S-EK_iIr4)s00000NkvXX zu0mjf^`-nvU}R*(3@Tc;ZVev=2g7jx02nbM5FS2!2&006AT%Td(lho;%&YkBUHKoR ztO(_t&8D5)rHdC6Pft&&@k&Zcm`PxHz}_T|{i>^LB*qk9Dv=~w3KN1lfb&<15up6s zSW6+75FBtJ2jUigL%h7CcOD;FZLPiSeKuf{WktVj5un4iTv7zVeGiT|1?wnb($d?Ni zFwb!mtZflegb-HsyK>nwFz0fivZ4ZNg=Wgj?w5fxpGQS#`1|iGyuP9}NGpZ<`g#&O zV+Q$T;snBTb!A%Z%F0U05v(!;{r$DV9hEpDfPsMld?%)4Hnz4fWY9p$iEk4UpzOf| zsI9I64-XF-|H6d}uZ$r|wd{g5DFH#}a&sv`rW_9B3=It6{DlkbT1CT9sf%*A zs0fzE#Zf&8^cxPozP_;Vi!b2Akt6AIw+poos>M~J!xg(e>=E33f9&K5kocEtrUjgj!+;X}B4=MF1eFee8uOc~uM7TOI`TR| zU;XMD^n~ad^mD)JMr=$B?EdK|kXMO1I6A=P{7c#)MA-@Kf2Zt&ja#-rd|Vvs*{@l@ z!uh;EVQ%b9Xa%ic_4n%~rlGcW1Q+Z0ZmwWvY6_pvo((zY&cVSWN8sm_6c`Z{#Kc2G z5aBFqs%xOQv=j;p3z;w+=Llkm7YB0W&aK<5{ME)0&+eYE?%Qu6GCGx0&pc;Fxj7x0ZaCdAyrf^1m7jxIBN{nb}YnzH7Mo%uY8;VmY_*3OP> z+x*vRHd+K1-zydw)r}rKiq5{^Auv`#R8?0qYaOUWL?(oWP}|t}rAw*FkpRMk)YjIL z`uYaV5rT%f+qP_>vyr!M-jW)xZM$|N_X1AEL`RdkvuD%!F=H`NQbn|sR`!aD3Tl)( zF*FpSX2ihmojZUpMFCyi+~95ZP9kAW4i3P3xiEwUnY?!|d@$^N*1K*U+>{&1Dai=R ziNvkVoVY226W;H>OBQ_oxk737>e-X{`}s+HhU<^+ojWu4Efy`*2_yn_0*Of3vqx%N z7e0>+8aP1Fq$zs7^9~g$BMFD0(7|)%w~P=}?8L2GX~H*d+>{!xkB>K*KX0B=M5qID z3BjrqiI_QkIwe9HC+g~65FVdL1`iylbs~r&gkb%S?S!!$PVo7BGGC~^*EVGKwK{a{ z2n!c1fZDoRC@Q)G@d*hMF*=AL81fyq!=diQ3)V}h6Fg^USgs*)QUxM}pw`FEm_e^iOif_JrcJ=Lun^_7qlC8hUa@zSL_MFdC^t^dzDMz;uLWgVl7o)<4R zo94Q@y28-GL!i2{63n<3uzBOASMj~lA+G&6p1?-%*P}=2{q@4@q@9BU88l!3-R`zM zF_BvFU@KquqF$pl7UJ_cb4aJo?##WywU+Pj;gk~(A3lQ6kdGl{?;d#a^eL=dxe5&0 zw1E`~2{0`p9IC1+!JKOjKWyBnn;emZ(LXPc7+)iq0Ybj^YxIj}-lp_ia ztoruv59_}Bj_&IzyjDQhQu+{p0NOkOILYrla5VD0MFlrz^03ZU|N1vvAZ z!O+MMjvP1$TrL;Ze!EI!ZcA%WMYKMV%1zFf2jqQk?XyN4%KR#vi}PmhRzy1H7Z zs{_bA|GPTAQ!Z2@1iLcvD^~)?*jR4d=*UQQf3rmBSFCgmdO~y!3Mo5w>kl73BoY=K z86j~ey*TnOU6#1LOcHu{^?(_%u__gjLQ8LGK8rgNj0_DmYUcFl@na~uExN2Wf9_mJ zJCF$lSFh1~oV6)x$E2z4R;C!v-GM!E=SPlOPjV@M94{pAa7wfIXc4aTesNn zDk5611N7Ceu0d}>r-x?n#z@wi0zE@?40?v>81xL$G3bnesjdOH+^Z99+O$z9H9bSb z5F0ZCHg4HU1C{N5N*j7QIy%sSxgyg8I?Irn>T1}Rk^;w%9fQIf*WvcvyYRfcg05Xy zaJkeZFEAhw#!r|CR@PQ6&JXkYV&Rv7gMhET{1WEQpD*Gd?tW=0oH%}5;=8G7sjx3C z9ojoN!3wdZ?)a4n@Ic@dRS~Fd0Yi;IZw6?*iR+c0goTl!qC2d_lXveXTuV#34ukf_ z1=k8BegL{H5t!y0$T?=k#7HFqU7KK>F5SA4ZXO;AAC}hE)NE0be+o&^DTbh%J=YFy zbP;UpX33tt#IZN|CmO$RA0LU&1q@jp7f0{WpTLr@zb0W5Cz2J*my?FZCb}q%rk9vb zMp~N0bdVv2Vy>ta0q(?o?HGa@ff9^%+h__c{_z{uttCdr#wQHE?7{9H9 z1GVcmH8Ue-=H~Q358Po<4d6=H}+i^ZE1ULD8MNFlN+fSoF_-iM)IJcC!{w=i1kesvG5IS)Jl$4af#9`gKrJ zTnf=Kv0!Uw0|i&E$kYf)K!-&^K_lV9pMOH1ci)BE5>`P~0gj@7OiqUPwrrt3uB>dV zVc3u%Fmb{Jk<pJYE!GHmXlQJt%vTrMv~C3sjtcsh zRUj{j)+^ln3$NF(LZL~yeWOpj5L&$mC+3l*b5psK)EeiuPa*2km7sy zh^w0$GmhA@X%ltQj`!U?J;^N<7=cdRo4cMIvS6NK8~)xDw1^=%vc~OiIE8@&x@Ak^ zsCJ+u^;@@YE1e!{1n#;-M!bsse!2jq8G^e(@n;J-e}n@%!Jc&iaN-+d|u;SKS9Nj9i0Zty!(I&PL$cR_D@3N&pZI(81iB&He zic9h=Ckb*Hg8TS!Z!|Ijt0QrZPz9(Fu2dt8>5fHde;IjQfR_4L_-L(9l^5u58%U5LGZ_gJeWN*MyEC`stf2Uv?Y+crjFb8 z#(WsWFr&J<8v6I^N4LD9^{0K?w&3aM3HcZErKXXQmPQAG=!MqU$Pmuwo_iyip$^Dp zNOesOw6kx^+~dkw?)g9H^?O5xK-bQl;aXuKoc!$=>s|ceP9Gm17#k7-V@8jrKh(pm z?cF>)bgUS32XYzW)4LaJ-<1SAx9_0$=pXjUqlfVQnl;ovN*IgpVdqA~clr^81D8Z!=@oE+gl`%>u6@%;RJ>L#M5ww5;j)B~2TcqP#^ zG$1fCfgV(>Urtcs&pPFyy1JH(2n=l2n6YEXix)3c&01qnr-o?F>SRaaHVBWN&KM-) zIXo(g9-yFMbK3q)FynGraWSp7ZSCRS9R>cArsL2WLmC^JXu-8^)e4?Hd!~?63q=t< zZ5pH>$ka3mTEUyKVdo8+iqY?&(lO{6qGQlAM8}|Kh>k&Ry)5)C%jfgq(UT`KcX5WofXDGzF6Nd1d?>bf7hwpmAyLjQrU&ZDF{F!{4j;g@5Q z_Y`UpEoMkrSs5fJCBcjOdS)dsEiH{2T2lzSrV{R+YS3ksI)I+mYiep>^{SOLwa+F^ z25TD|CgsBi55nckm+AGDYgg&Hs;ZjqcM)C@eJrQO5J!uRt=1.0.2 +""" +Get a zero configuration for a robot. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + + +class ConfigZero(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, robot, group): + if robot: + return robot.zero_configuration(group) + + return None diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..02520fe07ea3a8e6a5a5ee74cb250826c539ebda GIT binary patch literal 2074 zcmV+#2<7*QP)@xvc*_MO z6SM;C0pJ3_^E$-qR06NS^|8q#J6i;EO)kXxuD*GBG`zEEo7q9Aus5og$Jd%z27%Mb z!b*-882E{HDNo986U&m0qO~ctzk8ffSlKDq@CRg+U*l`?7i10SAoh zV96IwW&rMH|9|Pf0lIV$oHdlSUjP6A07*qoM6N<$g5>$yi!;(&w3SntP#M7ATaiq> zQm=_P*_m9qAMtxfO4_7MVkLFnU4_2EZt;|+#oCB!)vX*)-}OJs#Ey;5^y3iNxiH$v zjn^PIoK~LO7#H62H8&%P)^5XjaVCLx8;&%=#z>8d?(@*dIiRGSoH61&BT!YRfk7)c z-}OUClJUIp2YycxIjOpw`oEFNWJ5epl0wD0dX5&=(5_F6IAd^IJkjc-)1jDlaisVF z|Au+uc96*PTagaZuM(`Q_j~88OpLRtMM5-#Xa^~U~J+QACJk?tG!9Ae%`16()4*cSHpt)Z;V|e@QVX)mC!H zkYEcUYI(~+)sdYk#As8oD?t*N_(dYWTtP-^{oIbKmb3qsltT%q*5CdEN{AS= z))yrO$ri-!6C#hJz9gtW`7{2?EsH8?BUo3PYduLxPoYG$k_`<3Hiq~4JHCRJM$X>h zi~STyvrw)O*d}Oj%gir0W#h^fBv-XY-7aIV-0flqmqn4ma43L;T7|vX3F9`VUac8P z#WyFu>p(g(&c!+e3e^RH{gMORsR*HteG1~Sk?casMT^bQ15%xBQ&5|cCTyq%ySfL| zWJ6VEq!}BkqVb_A8>%8BEH;RxK;CFCJ+h~4QDgg$tw;9IroPKYY@wAk_@T&Tf}|fO%kYV;A;?HA64@B3XRbe^DRCG8WAr~nyKC;kkv)zC z4A8kIq3hhTs7g*X^2hWd8jr*`1#s!QCbcOBX(#@;lW{u@?Cte zek?_W5ZoWJbEdBuNlAluDnurY*25>G$GMggLP^hlL74ZE&7Es-Zz^lt9%f&Z-Y^T& z8)iXz!_o9w$AY8LtYg7UNN1P{=?pU=ona=VGqCQ3>=BN6ZwWzxt>(MErqro$GL%}x zY6tM#*##`++_0lApy(-dxefsjx<4vs^Vt}KdB;wQt_ZQd|IU?Qt#A*wJCxroBY-Xj zb?dSW`&L{stZn~K?_B9{7WJ(?|6p5yHH|aAW%GOt-YDD>9gGp^5o#oCb5H6MuCQ3x zOkt#x%jKNJ&;8Pp|pehA*@Tw#o=R)(bVPCy8!f^IA4wW02B@xu0~2}x)jDV{5fUKdfgnRAbd6$8eKE9bAzYu)WI`fj z6#J@*9t4S?J3>+yf(^)MidVgbD@2CKb-Yq|wS*j7%NXz~2@xhksKuV(=d~#KJ{Q zGECs-vb@mT3uOvISgoWo8Ps8OKQwneZN4ys0rGQ3Qc3q5&RF)P#f3&bCiBZmyotHB z2g+o=SyK|SfDNLPNK}!^f&4hWUo0R2WDZUpD!#Hub5sXqOM1!p_}6f30;^p_TcI>nfs`VZp`m+Rr~^u z6NK)4k-|NU6^mZTP(AL405L1)Ql6YCGKD+vq}PzgbYlg9S9i4rbZslg4&~dU21DQ$ zTtU05^bqE|PCCPUPQ5eCgmi|Pkj^j@(ivt#dV*5wKj)lBZ#Bfb4FCWD07*qoM6N<$ Ef`8+}WB>pF literal 0 HcmV?d00001 diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/metadata.json new file mode 100644 index 0000000000..32b547efa9 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConfigZero/metadata.json @@ -0,0 +1,31 @@ +{ + "name": "Zero config", + "nickname": "Zero config", + "category": "COMPAS FAB", + "subcategory": "Config", + "description": "Get a zero configuration for a robot.", + "exposure": 4, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "group", + "description": "If assigned, it will return the zero configuration for the given group, otherwise a full zero configuration.", + "typeHintID": "str" + } + ], + "outputParameters": [ + { + "name": "config", + "description": "The zero configuration." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/code.py new file mode 100644 index 0000000000..2318b67ac8 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/code.py @@ -0,0 +1,32 @@ +# r: compas_fab>=1.0.2 +""" +Create configuration target for the robot's end-effector motion planning. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + +import math + + +from compas_fab.robots import ConfigurationTarget + + +class ConfigurationTargetComponent(Grasshopper.Kernel.GH_ScriptInstance): + DEFAULT_TOLERANCE_METERS = 0.001 + DEFAULT_TOLERANCE_RADIANS = math.radians(1) + + def RunScript(self, robot, target_configuration, tolerances_above, tolerances_below): + if robot and target_configuration: + default_tolerances_above, default_tolerances_below = ConfigurationTarget.generate_default_tolerances( + target_configuration, self.DEFAULT_TOLERANCE_METERS, self.DEFAULT_TOLERANCE_RADIANS + ) + + target = ConfigurationTarget( + target_configuration=target_configuration, + tolerances_above=tolerances_above or default_tolerances_above, + tolerances_below=tolerances_below or default_tolerances_below, + ) + + return target diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..d721d6bc2efe3fc764c59075c3fe3c2bfebf5194 GIT binary patch literal 680 zcmV;Z0$2TsP);UVI~yF^RhiJ{Xdjym{Yy-+ON+ z8)aGIe@bs1+m1>Gj!Nz>qq`OWfC+$8Moz){HsfL9Isj*3{0h$@bzm9D<=KH%pEe%=P~y=7nJ zZ4t{$j;K}sP{iKkmf}4Cj|>O)`TaeBkSU@F&Wc!uO(1p-b^tu%=^Fr7idfE!?}CjI zm7h5mbBdyfrR`iJz?eOQ!K+{@Og#dLxDaOOkiQzuQAy68X*n!n8Fd-ylBPbeleXX1 zRDtkDf$&aN0w`ih8{S&Wla~FIlie9TWD**ZyrFmA%SL0E^ z74g!Jf`9|zQ4hT4wK+d2>ZerZf$(Z{nvJGCKLc>10oUF9_^yiT=hhB>+CK-p(`IV%3cEY*ZHlyfUHge{(2GYCuRE>>q zUr^rKLVt0L-udD&fIIk|6MZON({vJ*X`W4&x^URQ!9Nx2GEeg?1OY5Nd{=1.0.2 +""" +Calculate the robot's forward kinematic for a given configuration. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + + +class ForwardKinematics(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, robot, config, group, link, use_only_model): + frame = None + if robot and config: + options = {} + if link: + options["link"] = link + if use_only_model: + options["solver"] = "model" + frame = robot.forward_kinematics(config, group, options=options) + return frame diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_ForwardKinematics/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..4a6dd5dbe43242d3d9a9e79333065527b901654d GIT binary patch literal 2709 zcmbVO3se->8D6nIf>HC37C0w$95@Bk*?H{j&d#_i!a`utC5oUnCJnPQci9QMJDZt- zg zGk4~G_xr#9b?>fTn42{@IzC#b(@nN#XXL?WZ19>m9^TIetXJXV9)I?)Rh@3qgTZTz zZfh+>Ovsh;mubr!3phdc>UmLi0)3g+579cECAG}Y3&lV~oS;bZS<$}z2T?>4t>{vt zgK+q5z%6B0C}43#ZoW`aEU+S)Iv25&age|ZG#)AQdVDHZW<`g2IXDh}#!+M#q7_@w zw4gy`nPVYhlNEp%^(IUpX*0sGdeTG_2GeYWB1je|%s6Sl2$CaAoRLAoKNMP1L>HHr z@o?A{e6yl%P4ja&9tZ^V0fSyvig1!;AqRz16b2EPTJF>MGR&tYj4)&XRZt|qCdob| z$jCe863vQ2r6Cu*{s^s44TlK^3@_vTIH@OsE)4@kA%gRlD4t>CqJRSr@B*KvLM$1< z`rWc7t8V!Rs3X(gGXO*Da71j}R*TmgF`;UirO=IVKyHgx^UM7J&jYGlq6i?f6v|8p zdgJG83g9(a$(Lo%NTL>wNJc1HPaw%lC7&n<)Ra*RKnAY?D@qYC)+C7$=6s6c3F+^d$%wqh|0q}#IG3z=dFZg@<%={-R21hSR zA_pjwD7aV_qZop~Xp!bIr(mElz_?r{mw_gXbjV$}K0_8tf+Y}KAL<5C79huW@=*fZR;6Ef@UFgq@fDKZ!NyzxlxD0NF~FQCOC{7aquN_NCF>> zlLC)GZj7}{r;GZ^o{^THee~^3kGoIuv4=jqo?6sKBt_3T`SEk^ zC2V~2?aie#>{#@%fkpa1uHTHVPW;T*xTBHmSpU5JtNF`|->lzrtF1t~B>uS;2OHopA1Q%1her6lL2Y#}nIYPDUM$z3dh@KQL?eeMvKiG8dGl z&#LC$ty*%TW>!N~{fp)T<6A4e7bkV4P0h_}f2RFLNyEX8K~3po#}+=l@r`j)=8ZpI zu&HC-fIm97C85@IYTAUj-^Dz2V_na4U-qn;@NU#z+e6Ld(D)DT{dWUso5m+MR!!|b zH@?|b*mNk7tBbdtVct7XIH$RNNBkR=sQll)Cnl$4&-p@W-FxKT)+dke>TS_Wy*GDd zGZ!qLO-+B;lNZ&$W=f~6EpJOl?Szkc#!@}~V)VC@`d?m`6t%tK@!49}hMC>f*J2OH zCfBPcV`92qsIDHor~6N@R^5LF>si#FP(Ak34?n;9$SS6Mr*xFO5qIJ@uBu8;FV?v1 z`P$z;+JC+7NMp;xOFp~MI{Q*{W=m&peB9Rf>4Ez@#k9PrxL9ib@AoD>05aUh^39aJ z8`!INRAnU=_uUDdPox8BV;AI9q!)d&HEv%=|McUk z`Tcll>L-==HP29X=03T915!C-XO^et)Gt@zzdAJkz?}DqT}PMX%#_Db=g%%l`16IF zp2C-YrY)`?isk;5HuUqBs#nTWAKSW@x)v~De_OgUr{+ucq0^Suf8eFd`v=yZm{_>d z{9xi1?$zDKd1#*V(wYG~^1`{C!}@nVt@!8K8#TJipYN-0X(>3-{Oq7ozNjGZ)I>^P?8(P#Hd0mFZc5_@Fa3vN3fwb|Hp7aB pZw@9d?HP#Mf?qDK8?$0;YujU|YExgi7X0IA&&2 zFCMhL^w{+x2tDLnMC+wz^`^OO4*~5d^j289Lwl?QKT;1iX3AS8vW9HjEat(m`*z>F z-~0N_kWvc0l)fmwiArN}W4IT9s3ZW`Jp%xc*Q;j$ivVWgmMLlmh?_-60HV??fV5T@ z9~_`1>W9yuC2B)YRCmHe>$HF)8#r?AvkP01h!!T6FbkG2omeF*6ZthC!QWIeaW%~E zv_*N^qTsl4$#R_l+MY#aq5^;&0Gr!WbF15#mnp2+y8z|^R7G+6Q=*o5!~D=&&H?N( z<6Y^=jsOAYP1$|*5#Oq5Z1BiV9Aq&1eFPxAxyzhm!{~X-h7vh{F*$1%1sjFpm#t~)?$4~2{H`k{N zRoo0Vo?Oh<3eEkl^c;OC&RD{{9dExxWg`Btrt9cDSE>?>!V+fl!21B;r2e>me4Y6x z`Y-#RJ^C{+31D5DrU1ML@Rj|z(z7F7IF>MN?Yt{-rAJ+nCTobZtRb9$Zf6al0(zeH ztRdE#-?}9lH!#Np3;AEv&%HZ6h$@yanPl7_L)V@4RXssP&5O6L^op?nbU&EAekLuy zc=DKNx6We$1c3OiU}p^EkFzU19_PCf_mm7Y)ef~$3jYB=lVSJb+G6kk0000=1.0.2 +""" +Calculate the robot's inverse kinematic for a given plane. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + +from compas_rhino.conversions import plane_to_compas_frame + + +class InverseKinematics(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, robot, plane, start_configuration, group): + configuration = None + if robot and robot.client and robot.client.is_connected and plane: + frame = plane_to_compas_frame(plane) + configuration = robot.inverse_kinematics(frame, start_configuration, group) + return configuration diff --git a/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..8ba864791a74ec71ba0bbfc9a1692d0d0e82b4f0 GIT binary patch literal 2734 zcmbVO3se(V8V*t(^4PW2uB^2<4!A{Sl9^=km{}5mghvhrA%+#KrA%gSNF>ROGl4|V z+WOcYP*G~^c9pfRm2MvtUDs1vT`AaIThx}e?(W$=xE}0j5vylGyRBNpvo{b@>RRh* z&Y77zbN~B&-~YNdjU~ke$qBO(WHMQ@xzJ>V&y>&=9}DmOe*F#jm?{)jNHSUC^w1S0 z+tms!;)-3ia$mV+G0pNGIm7XGAP;y1Xf2avEeZ$>TLXNE9aOu#dUWX6J1FAf^yo6R z1-A%B;B*z%i(qMev5l>-VJQw>WI(b4Gz9PfAAGya*7rT!XO$sYA4soY0WCN|T8wae`9dIt8J^aDv7)v|5XVe<)-VIR|Yu z<%hZ8n;v!gd;+ad`2BvlUnS?oY6U@25JRa@Dluq*Nvpj+CV+XRv|efYSkh38z3)pwXuHXNsMl2&QIV+u=#V|>gH8gne|;6)Du9d>z`YM>Cj)hH4P zBW>i}yas39ng22;w3F}W5CZT3n&#>)b;Nsq$d z$XzauwrlNxAP5R09jpT*36j8c4y_tv8H&=9Iwhxas3+E&c(yiF0-^Qe-N5lI#Q0X8 zn$)NmO69;ft%k)k9I3?YoC;tZq16J4r0iOC1Z|1vf(^;IC!>Z`aS&0B;|$~mn9}ah zVOl#$VT_Ifn9k0s?ZBbbFoY&l_TMs3pyWXRB$#n2+*Y|vb$H}<87AO$Dn_m1U}iZ| z&0x3#!rKARVRjPNDYXox#uv+*c&R>-|muLGMwuyuZwdG34BV3^?@r^3?r>(TNXIKoacA+3%}qKKCMp@8G@)VSh=VVY zLlSsloJ4p82752TBQ`qLY|60}zSp+%@j_cp%6q-7wKd(J1kVgUwQ2C$G%&O9uygBQ zPOKmN>)^hRjHM;%T;pCW$1o%2-`Shj56*iC-M4Z7vj;O?*_*mQeoj;7>A&s7pYVUP zu5aC^HKi-BZkVHz23m&<(wU7*-#CxkpN^5b8VtMRJ4Him(l!0iNK4r&{(-9nC0?EX z)Y-0FeDmcf8fhAB|K_o$UW;ul=<6niQ?jNgvJ>ArslRH-Pa9a5c!k(f%*Ebxgk`G3Y zB9GkKJ3YC*F;E$o#{40TN~rF7=u&ghn!41(()p;PM;9*JeS08wNq&1@*R21%s5$w* z^znt$=Ptb7{9r{=Q&;rM?^AQ1m=S#bC!0>6C1T&cKI2N%=HV-4)?eH{{9@VVDa|86 z@h0LOIr>Qh+NM7LQNL|^QW?l!Zg^F7VZMIuo|f``Zyn6c8|nGrct4sEw>49+X{OyS z`q?ixJo@m-BabfrO`SOVa>h`~lk;a*^!~AhB~wSAkDeEozP99rsY!_|a^j*JgKaTd zX6>BTwic7^xqY9nDNla$c`l%ktv%KfTXNPp93p&awf45(}DMZcg zOK?i?kPCA8<_pHrIpf)^V46JBcJ4X;VTl7Zz{V|%+x zJGVz4+W*Y<)NRjxksS0NdVR~vUZYr?{qq|qcIB^l;8%I#Z`mDRtUCSX!*BPW`}N#~ z!oQO1u5~WI6g(Dxc#Uj#g?q`?owE-#q!qOuA4>K=hkcnD)A_CttuKo?>vm^bwc9GM z*UxY2w0z~7ZEF70?{6&_*paRJvXY(^J+<3&b7abYM{~Dq8m>2Fd|Id9ve9~|>W$IF zd6!dm-r_XE`40J!(nks}zVpuor>vG1vvt=Eb9$ri?_A|$Lss>0VsT&^=`2n!4*jDv M=M|eea;w(=4?ncd&;S4c literal 0 HcmV?d00001 diff --git a/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/metadata.json new file mode 100644 index 0000000000..27c7665a36 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/metadata.json @@ -0,0 +1,39 @@ +{ + "name": "Inverse Kinematics", + "nickname": "IK", + "category": "COMPAS FAB", + "subcategory": "Planning", + "description": "Calculate the robot's inverse kinematic for a given plane.", + "exposure": 2, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "plane", + "description": "The plane or frame to calculate the inverse kinematic for." + }, + { + "name": "start_configuration", + "description": "If passed, calculates the inverse such that the joint positions differ the least from the start_configuration. Defaults to the zero configuration." + }, + { + "name": "group", + "description": "The planning group used for calculation. Defaults to the robot's main planning group.", + "typeHintID": "str" + } + ], + "outputParameters": [ + { + "name": "configuration", + "description": "The planning group's configuration." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py new file mode 100644 index 0000000000..697a139c44 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py @@ -0,0 +1,51 @@ +# r: compas_fab>=1.0.2 +""" +Calculate a cartesian motion path (linear in tool space). + +COMPAS FAB v1.0.2 +""" + +import System +import Grasshopper + +from compas_rhino.conversions import plane_to_compas_frame +from scriptcontext import sticky as st + +from compas_fab.ghpython.components import create_id +from compas_fab.robots import FrameWaypoints + + +class PlanCartesianMotion(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript( + self, + robot, + planes: System.Collections.Generic.List[object], + start_configuration, + group: str, + attached_collision_meshes: System.Collections.Generic.List[object], + path_constraints: System.Collections.Generic.List[object], + max_step: float, + compute: bool, + ): + key = create_id(self, "trajectory") + + max_step = float(max_step) if max_step else 0.01 + path_constraints = list(path_constraints) if path_constraints else None + attached_collision_meshes = list(attached_collision_meshes) if attached_collision_meshes else None + + if robot and robot.client and robot.client.is_connected and start_configuration and planes and compute: + frames = [plane_to_compas_frame(plane) for plane in planes] + frame_waypoints = FrameWaypoints(frames) + st[key] = robot.plan_cartesian_motion( + frame_waypoints, + start_configuration=start_configuration, + group=group, + options=dict( + max_step=max_step, + path_constraints=path_constraints, + attached_collision_meshes=attached_collision_meshes, + ), + ) + + trajectory = st.get(key, None) + return trajectory diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..f401521a5d2e6d71bbff57dbea3d0d6a3ea761f8 GIT binary patch literal 1594 zcmbVMO=uid9G_LAHf1l>G*XL>Qx8_1&)rNi&rI8NC(W$s#*nNwJqVe+dAmFA%)IH$ zY<81~ELtO?U?5r=jl~MJA|{G>2v%u5DYo<`q6ZP75D|@@^iaV!`!S>vEq36|`})1# z|NH;q=*UpAb5|!tQOT^Xjgk4TXxx4)`TtN=|0L5kPd^$^)b=}~k)U2*CWwxmR(>L! z$Q=d{JDg$SDa6$rkDw{4e}Bz0U>Sw<6e?P-%50oF&(M~sGDmuHLe9&elBLi3Xnb}g z4`<6zF`51Q=>8fY1P%%fy5`t!0BS1J;00tI9rFy`fP`h08HfbZ6S+}3gMCEHoWMds zkmz286XkST79OT0K@xbOhZiMQl!1^2VlUnL7$VI#3t&t;)RIMZDpLwW5Ab}oTIH%K z4*Nx3R1}35BwmtOf?$IgH#BOj8|-N_XefZb<%Je@>4?#o!j({Eh^9>w951GIgO;0! z!}yxv@ggTgCN+R2jB#GYw;RGu$Ris$$PEL66=SSd!XXYy_+O~w>uUrM*XDAuj4gF> zoLEE<4o(v@S`OI~9pq;`#E+o>SA2*Dr-_<-B5OR5@sSZ?Kaa894%BE{Wm*zBiGJXS z<(jw}?A`PLX-0@thD1$d1&I}U@}dM%GLV#q1Q7^A6Pm-ORhZcl+6TnGYoH`$Od~X| z3pODrVBax_!Ionb5%0N0hK}6`GT6pG5lpm8Z9dOtGNV2&ST?x`#)b~k*};q~DzeN< zoY+t|mjhWh2n`patfn%=IhY-!M;9m{INk-3mxlXIiB*j*hifwnaD61sE_Y1C@bM{E^AD zj;$N^K$EswLWJ}`@}da~a3QQ3J{l+zZ(TRQTE{+}i`?lu*78%{b#w2WI7 z*~HBbxunDSq?J9odNpc;UuCs{y#B>6uMKxSC)}xha9lrd(+=kJj)nSh{oPycrxV-A zwEI(c{?u~%+l6mdCoWC9(#8u*zYia~dEcSmD0y$<^CWe+-t}F7VypXh- zXGMRq{{F%{`j1P0oO%4Ty_lwgC%*iw+?^Oc^UBbbQ=go8^W~=&*VgQDR9<=hN}}}F qWaZp#E4$Y2S^nsyU*EXeec^6>?(F6H2On9F%*qaqXsZV&pZyzN*$e9c literal 0 HcmV?d00001 diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/metadata.json new file mode 100644 index 0000000000..bced52a014 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/metadata.json @@ -0,0 +1,60 @@ +{ + "name": "Plan Cartesian Motion", + "nickname": "Plan Cartesian Motion", + "category": "COMPAS FAB", + "subcategory": "Planning", + "description": "Calculate a cartesian motion plan (linear in tool space).", + "exposure": 4, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "planes", + "description": "The planes or frames through which the path is defined.", + "scriptParamAccess": "list" + }, + { + "name": "start_configuration", + "description": "The robot's full configuration, i.e. values for all configurable joints of the entire robot, at the starting position. Defaults to the all-zero configuration." + }, + { + "name": "group", + "description": "The planning group used for calculation. Defaults to the robot's main planning group.", + "typeHintID": "str" + }, + { + "name": "attached_collision_meshes", + "description": "A list of attached collision meshes to be included for planning.", + "scriptParamAccess": "list" + }, + { + "name": "path_constraints", + "description": "Optional constraints that can be imposed along the solution path. Note that path calculation won't work if the start_configuration violates these constraints. Defaults to None.", + "scriptParamAccess": "list" + }, + { + "name": "max_step", + "description": "The approximate distance between the calculated points. (Defined in the robot's units.) Defaults to 0.01.", + "typeHintID": "float" + }, + { + "name": "compute", + "description": "If True, calculates a trajectory.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "trajectory", + "description": "The calculated trajectory." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py new file mode 100644 index 0000000000..f7717c31f2 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py @@ -0,0 +1,47 @@ +# r: compas_fab>=1.0.2 +""" +Calculate a motion path. + +COMPAS FAB v1.0.2 +""" + +import System +import Grasshopper + +from scriptcontext import sticky as st + +from compas_fab.ghpython.components import create_id + + +class PlanMotion(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript( + self, + robot, + goal_constraints, + start_configuration, + group, + attached_collision_meshes, + path_constraints, + planner_id, + compute, + ): + key = create_id(self, "trajectory") + + path_constraints = list(path_constraints) if path_constraints else None + attached_collision_meshes = list(attached_collision_meshes) if attached_collision_meshes else None + planner_id = str(planner_id) if planner_id else "RRTConnect" + + if robot and robot.client and robot.client.is_connected and start_configuration and goal_constraints and compute: + st[key] = robot.plan_motion( + goal_constraints, + start_configuration=start_configuration, + group=group, + options=dict( + attached_collision_meshes=attached_collision_meshes, + path_constraints=path_constraints, + planner_id=planner_id, + ), + ) + + trajectory = st.get(key, None) + return trajectory diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..ae7dad3dbad5ffe32c035a62e9a9b296f7f24b70 GIT binary patch literal 1701 zcmbVNZD<>19DmYk-#ZyjaYgHSowLp39KE$^Qk#hBLK^8CNH2F!(xbV1Zg(C=@LW^|>?+om7WAxaXdi z-}C?d-|yu}M|)*iRT%)FG87C%@%I7yEh)m|Qd;-}e@gXWj|o8ez4q$>lkZ@}-S;c8 zZmT=|G%q35Dat4gof%cfXaJg;GrA}ZLW_vQq@oGr>e+K7p~wQ+RUc-;x*rZG!BGQt zj&{VP(Lu>8lg;~yrVNh-sL&FLjGEF+J|mD>xjdfR+cZgJA=aQk`fNg?I~*bW$bf{$ z$xsr*a72UG$$H#w53`Tp7>=QtdYa`Z*26Pyo^2p<4~f|fIl)H*Pv*GrN+1U;OXq1i zolZN`E+;aQH0$+xX@;XYj=~7a9MLQ>LuqEsri1`AB}36I1!;sWQH-M@OCYhP8z!iF zUaV&3+=LxQXGEQ5os4Z#7AQ-3oIYfvvgEQv!xU7ZW|hAI47G`Hyq|enrVre1VaIV#LjUlip(cmkd@`e1m)tq9?A>daVjq30Yb^` zWf~ZnlQ2;>VtfEeLv{*m-k1O9{R4)AOHfSRu^hWF?cCvmiix8-k}I7~crZ6hDMU71 zcu}&;Kp-VM{!k`!+sYk#U_)$r0Aiv4krx{<6D6#)Xh2^Qd+WA&M`Pt_yI?o$p#N!N z_H0XWZ^CiMu&4P}g%|l|2Q}Q`4BX20KKIrf0FL@lz!wWH+<5)0TF6!$J-2gi+;zA( zah2IxTo*g?<3OrTW+ literal 0 HcmV?d00001 diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/metadata.json new file mode 100644 index 0000000000..2549aa96c2 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/metadata.json @@ -0,0 +1,60 @@ +{ + "name": "Plan Motion", + "nickname": "Plan Motion", + "category": "COMPAS FAB", + "subcategory": "Planning", + "description": "Calculate a motion plan.", + "exposure": 4, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "goal_constraints", + "description": "The goal to be achieved, defined in a set of constraints. Constraints can be very specific, for example defining value domains for each joint, such that the goal configuration is included, or defining a volume in space, to which a specific robot link (e.g. the end-effector) is required to move to.", + "scriptParamAccess": "list" + }, + { + "name": "start_configuration", + "description": "The robot's full configuration, i.e. values for all configurable joints of the entire robot, at the starting position. Defaults to the all-zero configuration." + }, + { + "name": "group", + "description": "The planning group used for calculation. Defaults to the robot's main planning group.", + "typeHintID": "str" + }, + { + "name": "attached_collision_meshes", + "description": "A list of attached collision meshes to be included for planning.", + "scriptParamAccess": "list" + }, + { + "name": "path_constraints", + "description": "Optional constraints that can be imposed along the solution path. Note that path calculation won't work if the start_configuration violates these constraints. Defaults to None.", + "scriptParamAccess": "list" + }, + { + "name": "planner_id", + "description": "The name of the algorithm used for path planning. Defaults to 'RRTConnect'", + "typeHintID": "str" + }, + { + "name": "compute", + "description": "If True, calculates a trajectory.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "trajectory", + "description": "The calculated trajectory." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py new file mode 100644 index 0000000000..171b005668 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py @@ -0,0 +1,21 @@ +# r: compas_fab>=1.0.2 +""" +Create a planning scene. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + +from scriptcontext import sticky as st + +from compas_fab.ghpython.components import create_id +from compas_fab.robots import PlanningScene + + +class PlanningSceneComponent(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, robot): + key = create_id(self, "planning_scene") + if robot: + st[key] = PlanningScene(robot) + return st.get(key, None) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..fb9f75869f98aa6ee09d3cd7d4f045a734010118 GIT binary patch literal 1706 zcmbVNe{2&~9KWt^?gwMjC1X(-=glw_uJ`WR9_@wNZS7cV?AB}zjNy-6?_S%>+PmA` zm3D(b!_-V785!9>#zg*t8RGz_hQvgSkZ59zKVtMBHBk6~D2FyJ3VbDKG8YtVZso1(=j+U<5zfT0+MLjp0iFzELX$T>Di5;=hdQ9jB>Nf#7Y(&cc7q{}X{ zq~LUeDD80B9lWc2->(R1GX=``#s7JKK$B1j^2tZ$F$>eo9WEg0D5?{s(iwu|rBzbG zi|NAgf>{P0LNMbGMWVDVJ+cSZX)_6kNdF@*)?vC5GcvpeyW_}P56wFYDNmUNyKV>d zZxf4W8;ZLOM;*glR$3MMs5Cn$qYkH`RyKP6MHa&>T+rVg2{a1%Lcwb#>8tbAk;Sfa z%hOfOTd>=foga2xs_yb_%3ZchQ`qg}yYp4gd~<6ATwgg;2tIw`?pM64ZR*CBSB@S# z_44-Bri1A_g)bYv%jMm_GR=)G`k|w1SFWB|9QgWE>`mwS_QGqIum4<_@gAF!ME4tY zeKX;tWz+T}tCmCIZEf$juC)K{f4lm}ZAbb~i?K-Sv4Yw6_FV~OEe;g)sy`{pwzcbpyt z`))ir*VOg=1.0.2 +""" +Create a point and axis target for the robot's end-effector motion planning. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + +from compas_rhino.conversions import point_to_compas +from compas_rhino.conversions import vector_to_compas + +from compas.geometry import Point +from compas.geometry import Vector +from compas_fab.robots import PointAxisTarget + + +class PointAxisTargetComponent(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, point, target_z_vector, tolerance_position, tool_coordinate_frame): + target = None + if point: + # Convert Rhino geometry to COMPAS geometry + point = point if isinstance(point, Point) else point_to_compas(point) + target_z_vector = target_z_vector if isinstance(target_z_vector, Vector) else vector_to_compas(target_z_vector) + + target = PointAxisTarget(point, target_z_vector, tolerance_position, tool_coordinate_frame) + + return target diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PointAxisTarget/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_PointAxisTarget/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..0613d36bb28ba43396ababe26de3bb32bbe036c7 GIT binary patch literal 620 zcmV-y0+aoTP)2 zFCMhL^w{+x2tDLnMC+wz^`^OO4*~5d^j289Lwl?QKT;1iX3AS8vW9HjEat(m`*z>F z-~0N_kWvc0l)fmwiArN}W4IT9s3ZW`Jp%xc*Q;j$ivVWgmMLlmh?_-60HV??fV5T@ z9~_`1>W9yuC2B)YRCmHe>$HF)8#r?AvkP01h!!T6FbkG2omeF*6ZthC!QWIeaW%~E zv_*N^qTsl4$#R_l+MY#aq5^;&0Gr!WbF15#mnp2+y8z|^R7G+6Q=*o5!~D=&&H?N( z<6Y^=jsOAYP1$|*5#Oq5Z1BiV9Aq&1eFPxAxyzhm!{~X-h7vh{F*$1%1sjFpm#t~)?$4~2{H`k{N zRoo0Vo?Oh<3eEkl^c;OC&RD{{9dExxWg`Btrt9cDSE>?>!V+fl!21B;r2e>me4Y6x z`Y-#RJ^C{+31D5DrU1ML@Rj|z(z7F7IF>MN?Yt{-rAJ+nCTobZtRb9$Zf6al0(zeH ztRdE#-?}9lH!#Np3;AEv&%HZ6h$@yanPl7_L)V@4RXssP&5O6L^op?nbU&EAekLuy zc=DKNx6We$1c3OiU}p^EkFzU19_PCf_mm7Y)ef~$3jYB=lVSJb+G6kk0000=1.0.2 +""" +Connect or disconnect to ROS. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + +from scriptcontext import sticky as st + +from compas_fab.backends import RosClient +from compas_fab.ghpython.components import create_id + + +class ROSConnect(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, ip, port, connect): + ros_client = None + + ip = ip or "127.0.0.1" + port = port or 9090 + + key = create_id(self, "ros_client") + ros_client = st.get(key, None) + + if ros_client: + st[key].close() + if connect: + st[key] = RosClient(ip, port) + st[key].run(5) + + ros_client = st.get(key, None) + is_connected = ros_client.is_connected if ros_client else False + return (ros_client, is_connected) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..01236afb81b25c59ea6feb65abecf73e83f7e6a2 GIT binary patch literal 1805 zcmbVN4Qvxt94{Zr#sUndWKqE5iev_^_tCHQSQ*e(TCp&?Hf+(L*SpvDXz#8wAz={ZMJIZ>?Es4KoD*GW^%H*C1!BB+i2`bu z%nPzh;;e&1t7jlpDH;+8Kx2@UkPuZm;v|KUHkz=|q#emWD3qpf5jx<0A}ZXM!LX zXhk*eCg?Cc#mG2mB6O3oK#nco-QIA+))=nD2PxsPa`xV?~ps9)nHl%cmSwUgTew&BUICAL!Mwmk0WOPC6e+loc1a0Ifr6oS=E3_um#apRB76cK zsDU}N5ue9pBONvyMw!U0x_&?H6IG27S>SU!QRo~K&vUc`W+-K@ti*sdY{o2%#fpV3 zHV)&=4m(w8H&YgS&RlrkEwK%H3Jl!m{^$Mq3J*(=N!&A!UYL6B&|Y4JQBCJdrxrY) zUnO`Xn=UlN>Sf?WSv~%ML-X7GJ$oQWn~VWS`X70bgQ-$POEL=^p8(5m1^q1gcub~pvLvTfnc9}R{fTYc_XLGM==L!@|UX6e;2 z-6Q&kT^V+%_|}kJ6~glNhfE!H?^LXt`%K5F>J^W6Qxm*mM-SEY%ejxIZryUF$Lkwg zJl8n6b$V-CYjX5kfrHPYsgXC=uQDcb|qjt4&4S5kDbaeC%j>7JeAOU~>$ zx3|4%xb5?h(EQExTSYsze0@wBv9|AW<(O2<@%_7ppT(&vf@@whZjp;t)^+k=7doRqaIuQHQ)m^gp Xx9&e)Se0qge^@@xJom2I&o%u8B4csd literal 0 HcmV?d00001 diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/metadata.json new file mode 100644 index 0000000000..1bb60ac4c0 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/metadata.json @@ -0,0 +1,40 @@ +{ + "name": "ROS Connect", + "nickname": "ROS", + "category": "COMPAS FAB", + "subcategory": "ROS", + "description": "Connect or disconnect to ROS.", + "exposure": 2, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "ip", + "description": "The ip address ROS. Defaults to 127.0.0.1.", + "typeHintID": "str" + }, + { + "name": "port", + "description": "The port of ROS. Defaults to 9090.", + "typeHintID": "int" + }, + { + "name": "connect", + "description": "If True, connects to ROS. If False, disconnects from ROS. Defaults to False.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "ros_client", + "description": "The ROS client." + }, + { + "name": "is_connected", + "description": "True if connection established." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py new file mode 100644 index 0000000000..b25de8827a --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py @@ -0,0 +1,28 @@ +# r: compas_fab>=1.0.2 +""" +Load robot directly from ROS. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + +from compas.scene import SceneObject +from scriptcontext import sticky as st + +from compas_fab.ghpython.components import create_id + + +class ROSRobot(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, ros_client, load): + key = create_id(self, "robot") + + if ros_client and ros_client.is_connected and load: + # Load URDF from ROS + st[key] = ros_client.load_robot(load_geometry=True, precision=12) + st[key].scene_object = SceneObject(st[key].model) + + robot = st.get(key, None) + if robot: # client sometimes need to be restarted, without needing to reload geometry + robot.client = ros_client + return robot diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..e28d24443696c10ec1aaf7e5ee0f719d085a49f2 GIT binary patch literal 1668 zcmbVNe`wrP98XJEyLJaF`{67iF;&@*CV9#IXz$sqYwxbTV_tW(r>)LHbIH5A*h^lP zq`hm0uCopu7MMx=pw`!l9+avknWEs=n%FL z9pC_~03g!MK~4zxd;!ozi$DZC@biMm3IPawP-v!04@1~2H4S^hoh2^vB{5mYF(J?A z^LZ}s<*=3Eg9MX~D*aRz-v1S%K*v{gA5nVpMhJd&> z9xroTR~KC`Q`k;qgqTrs$aT?na@0h853=#FrJ%?NQM1vt#)KgY$qu%X80QuPl~`1n z76neE*Y|0Liu3k{B^OXwc96u7s0l0(S>R6!0Q!8;@7n|f2*3g~j#Vu^`cJ3`y)96< z7D`e^l^ywpU{!%>Z0RyFSkvVU;!PvN&}BEm5YAzX5EJdZOZQ`;P{P7#Ek_RQo~{l$ z76}D}U?9MXoKRFZ9)~f*c4R|Av9QDt=Wv>)LSH)ISAe&L^?Q{x+v4{DHsxJ2KQ+#(aAV#Ht=E5mLIEZ$fD=lyM#MoLi5-LxFHFx}jNQOzb%9W9kkFWOOBKq7GLRU>jXzSE(zbTf9#{~Y&mto9Kk{M$X5+Mzmo3zuA>O)S-tk0v-YwV#JNUnw zSUg)&+>3D1G2CUjRgsT!vqJ{ya29E0m8YJ%NKqB5W8wB>^vq}bPkSc7ipXud>svcI z2dg>u>on~Fcz zzs-GerRkk{|3Lriux|d5G5*8d_usSQE5G%VIdc2f4dc&LC8u6ncK)}yJ@vCMHB)Vu zANu*NodZ`+P6TF?Pcv&y&OCg8yJN*n#q>wi#)|2wTdL2cj_td%L6yH8-=$}_*G^AA z`{K@R^%Hxa-_ubo7xq_Hg|=1.0.2 +""" +Publishes messages to a ROS topic + +COMPAS FAB v1.0.2 +""" + +import time + +import Grasshopper + +from roslibpy import Topic +from scriptcontext import sticky as st + +from compas_fab.backends.ros.messages import ROSmsg +from compas_fab.ghpython.components import create_id + + +class ROSTopicPublish(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, ros_client, topic_name, topic_type, msg): + if not topic_name: + raise ValueError("Please specify the name of the topic") + if not topic_type: + raise ValueError("Please specify the type of the topic") + + key = create_id(self, "topic") + + topic = st.get(key, None) + + if ros_client and ros_client.is_connected: + if not topic or topic.ros != ros_client: + topic = Topic(ros_client, topic_name, topic_type, reconnect_on_close=False) + topic.advertise() + time.sleep(0.2) + + st[key] = topic + + self.is_advertised = topic and topic.is_advertised + + if msg: + msg = ROSmsg.parse(msg, topic_type) + topic.publish(msg.msg) + self.Message = "Message published" + else: + if self.is_advertised: + self.Message = "Topic advertised" + + return (topic, self.is_advertised) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..8ab79bac520aeb4ebbd73a2ca6db032987469146 GIT binary patch literal 1081 zcmV-91jhS`P)SGeVt&M@2;vrkKSlHZpnzyI@l zd;YMDW&CHL*A5+fulu*bx3+20%qQPJco<<=%+cDfPjvj;7g?#zAFqA)^_OR?j@)-@ z&mPx}OcL93Hhy{K+^3%q^(YJg-Ufh507wotq6dIR05~O7i$00K?(~k-Fbo$xG-}%9 z^3quh%RM(ZYTDrOGEoCVhRr-Ll-Vd(;izFc-b1sow|a+<5cOysiO6vPI0gVIQlr60 z33B~Xj#NNaQ*-2ihi1v0+cK# zD#%SNCnPFKVp0$SJ#sl7I%c#anXQ!ghE=;q5QPsGi(lW>28rQ`FzC=hZfnlsvweuU zJ@hlC>Df}J+m|83F@#|#+~x7@G)>KBdudiB4L-lHK+`mvKsi0HiXc|~oA|JE2zGar zFZD#F%n^t@|HU^yWKCP_St&{JUAzzoulwq|3w5U(^Ei%AT@xSE(m!NedFfi$`PTM3 zyB%qRk+e5HJ-*ntNNbHeOaLNrN0G!U+Nod#OBcG%GFX z(%OU=3n?d3SExc|mZt^;0r)WgydSQcTA?_u1lC7vfWjwtK#Qpv*Z{j&UyqCvCROYz zwF&@c94@aY?L11o0tg&ebG*Q~y);MQ7=(o?L*yhGzVc$^n?Wl+atFO@1fFf)@{c3S z3U+CW2ZkpO0Rly2gldYwam-+vInaGqm#rX(Cse9X2SPv(=}nnE_y4HUD0A-KVB0i7 zD}qn(LhED;#4!mApZA<{PXvsLptunL_Q`np>L0)TzBAuswr@{f7xxRx_#G$e8!w-3 zXx;1d_;O1MHn*9r6mj(9a~H2PbyWpCVyYqj_va;_KP(dPL9@LHEfFsgszocNADDDtoADjD1MgA>>kZny|G*(`uIeYVV z|Ehg2>=0>ao@Izx|SW00000NkvXXu0mjf&MfjY literal 0 HcmV?d00001 diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/metadata.json new file mode 100644 index 0000000000..a57edc2076 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/metadata.json @@ -0,0 +1,42 @@ +{ + "name": "Publish to topic", + "nickname": "Publish", + "category": "COMPAS FAB", + "subcategory": "ROS", + "description": "Publishes messages to a ROS topic.", + "exposure": 4, + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "ros_client", + "description": "The ROS client." + }, + { + "name": "topic_name", + "description": "Name of the topic to publish to, eg. /joint_states", + "typeHintID": "str" + }, + { + "name": "topic_type", + "description": "Type of the topic to publish to, eg. sensor_msgs/JointState", + "typeHintID": "str" + }, + { + "name": "msg", + "description": "The message to publish. It can be an instance of ROSMsg, a Python dict, or a JSON string." + } + ], + "outputParameters": [ + { + "name": "topic", + "description": "The topic instance." + }, + { + "name": "is_advertised", + "description": "Indicates if the topic is current advertised or not." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py new file mode 100644 index 0000000000..3e83b13403 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py @@ -0,0 +1,82 @@ +# r: compas_fab>=1.0.2 +""" +Subscribe to a ROS topic. + +COMPAS FAB v1.0.2 +""" + +import time + +import Grasshopper +import Grasshopper.Kernel +from roslibpy import Topic +from scriptcontext import sticky as st + +from compas_fab.backends.ros.messages import ROSmsg +from compas_fab.ghpython.components import create_id + + +class ROSTopicSubscribe(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, ros_client, topic_name, topic_type, interval, start, stop): + if not topic_name: + raise ValueError("Please specify the name of the topic") + if not topic_type: + raise ValueError("Please specify the type of the topic") + + if not hasattr(self, "message_count"): + self.message_count = 0 + + self.interval = interval or 25 # in milliseconds + self.is_updating = False + self.is_subscribed = False + + self.msg_key = create_id(self, "last_msg") + key = create_id(self, "topic") + + last_msg = st.get(self.msg_key, None) + topic = st.get(key, None) + + if ros_client and ros_client.is_connected: + if start: + self._unsubscribe(topic) + + topic = Topic(ros_client, topic_name, topic_type) + topic.subscribe(self.topic_callback) + time.sleep(0.2) + + st[key] = topic + + if stop: + self._unsubscribe(topic) + + self.is_subscribed = topic and topic.is_subscribed + + if last_msg: + last_msg = ROSmsg.parse(last_msg, topic_type) + + if self.is_subscribed: + self.Message = "Subscribed, {} messages".format(self.message_count) + else: + self.Message = "Not subscribed" + + return (topic, last_msg, self.is_subscribed) + + def _unsubscribe(self, topic): + if topic and topic.is_subscribed: + topic.unsubscribe() + time.sleep(0.2) + + def topic_callback(self, msg): + self.message_count += 1 + st[self.msg_key] = msg + + if self.is_subscribed: + ghdoc = ghenv.Component.OnPingDocument() # noqa: F821 This is defined by Grasshopper + if not self.is_updating and ghdoc.SolutionState != Grasshopper.Kernel.GH_ProcessStep.Process: + self.is_updating = True + ghdoc.ScheduleSolution(self.interval, Grasshopper.Kernel.GH_Document.GH_ScheduleDelegate(self.expire_callback)) + + def expire_callback(self, ghdoc): + if ghdoc.SolutionState != Grasshopper.Kernel.GH_ProcessStep.Process: + ghenv.Component.ExpireSolution(False) # noqa: F821 This is defined by Grasshopper + self.is_updating = False diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..02400a6c4107a24b288174f715395e803ac883c0 GIT binary patch literal 1098 zcmV-Q1hxB#P)T;@)O{hQjS=5e z42E)1ic|=QKFAVNLoA4iEl9V!-P+E!)5~@*-AiY8W_D(}J7&z%wCV%C629al|9|rT z-^uyT1s>ty2a;v^fs@qh_5boAlNkp9EPj>bj+!Ne*lTZm_+EBNi);T|?fqVlyL@)m zyQ8(O!~Vqjo!)njetw%{nGA{=-?L-i`*d1ft#j9bBOf4>86lJT05X|J6;c2Y;ub+B z^95uwXDkG`-9OaEal#7gFkKW3v85a@Y#JP<^O+ErHejVTh@!O3;|snxOig7lET4X> z-*y-PPRDIE0Pp}H#`)9$@CpFjK_+vF;!w4<9X`mgJh|ohVqazXx)Y=tcLe9c)gd-Q zHr75j`%K}QtE7gQ^94eMBuPY6mGz@;S-V~Nti|M5=d%g;>*YE4)nPwKu?1xk0H7XK z>^XzNOmH_B5p<-QnBaNg2EtH8t09YcLCVpR#JD8Q_o>x*;)uhUYoGFAhit?z0FbD# ztTt%i=e{+o%!Lb8IEF9`B?p=C7JAZM7GilF zAb@Br#2%<47W99evtd5j(mwQfKzd5jt$y&Rli)O+i^iD9)n4b#S zdJSyKTm}~`9*C=ziuL|xiOzTeaK|#LgcdOj)!(&@Bu%?~Fko}!I9)!&AA?lEfPE}) z!s&gWc18#-oK>HE+|i5>a1C3_?`|wz+ojc#lo}_J2?9@QsjnO^eLDACcIHYasV0mX zZGzgN&ss7J#fmh|1xxC{&o5N_PvjV*b?M6uIwf0wFeEFQrx}(3f#U{2l(@MND-bva zNr~EoDIQXaI|uFmr^u@6n#S4z+t?vMph?y1CJ7wJD0=2#@9&l}4M9An(AGB8J{))Kx^D6 zAqpTx7uJt5(cJ)0BzM&GDGvP2{S8OYetW&CadVlctoSLLjAZZ2bsJBF!+gc74OO=g zLY}NuE3o>lgYbp QI{*Lx07*qoM6N<$f~U_05&!@I literal 0 HcmV?d00001 diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/metadata.json new file mode 100644 index 0000000000..9a66d925e0 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/metadata.json @@ -0,0 +1,57 @@ +{ + "name": "Subscribe to topic", + "nickname": "Subscribe", + "category": "COMPAS FAB", + "subcategory": "ROS", + "description": "Receives messages that have been published to a ROS topic.", + "exposure": 4, + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "ros_client", + "description": "The ROS client." + }, + { + "name": "topic_name", + "description": "Name of the topic to subscribe to, eg. /joint_states", + "typeHintID": "str" + }, + { + "name": "topic_type", + "description": "Type of the topic to subscribe to, eg. sensor_msgs/JointState", + "typeHintID": "str" + }, + { + "name": "interval", + "description": "Interval in milliseconds at which to expire the solution to propagate received messages. Defaults to 25ms. Messages are not queued, so if the interval is greater than the rate at which messages are published, some messages will be dropped.", + "typeHintID": "int" + }, + { + "name": "start", + "description": "Start receiving messages.", + "typeHintID": "bool" + }, + { + "name": "stop", + "description": "Stop receiving messages.", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "topic", + "description": "The subscribed topic instance." + }, + { + "name": "last_msg", + "description": "The last received message." + }, + { + "name": "is_subscribed", + "description": "Indicates if the topic is currently subscribed or not." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py new file mode 100644 index 0000000000..5983fcbb06 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py @@ -0,0 +1,156 @@ +# r: compas_fab>=1.0.2 +""" +Visualizes the robot. + +COMPAS FAB v1.0.2 +""" + +import time + +import Grasshopper + +from compas.geometry import Frame +from compas.geometry import Transformation +from compas.scene import SceneObject +from compas_ghpython import create_id +from compas_rhino.conversions import frame_to_rhino_plane +from scriptcontext import sticky as st + +from compas_fab.backends import BackendFeatureNotSupportedError +from compas_fab.robots import PlanningScene + + +class RobotVisualize(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript( + self, + robot, + group, + configuration, + show_visual, + show_collision, + show_frames, + show_base_frame, + show_end_effector_frame, + show_cm, + show_acm, + ): + visual = None + collision = None + collision_meshes = None + attached_meshes = None + frames = None + base_frame = None + ee_frame = None + + if robot: + show_visual = True if show_visual is None else show_visual + show_cm = True if show_cm is None else show_cm + show_acm = True if show_acm is None else show_acm + configuration = configuration or robot.zero_configuration() + + robot.update(configuration, visual=show_visual, collision=show_collision) + compas_frames = robot.transformed_frames(configuration, group) + + if show_visual: + visual = robot.scene_object.draw_visual() + + if show_collision: + collision = robot.scene_object.draw_collision() + + if show_base_frame: + base_compas_frame = compas_frames[0] + base_frame = frame_to_rhino_plane(base_compas_frame) + + if show_end_effector_frame: + options = dict(solver="model") + if robot.attached_tool: + options["link"] = robot.attached_tool.connected_to + + ee_compas_frame = robot.forward_kinematics(configuration, group, options=options) + ee_frame = frame_to_rhino_plane(ee_compas_frame) + + if show_frames: + frames = [] + for compas_frame in compas_frames[1:]: + frame = frame_to_rhino_plane(compas_frame) + frames.append(frame) + + cached_scene_key = create_id(self, "cached_scene") + + if show_cm or show_acm: + cached_scene = st.get(cached_scene_key) + if not cached_scene: + cached_scene = {"time": 0} + + # expire cache if the component has not been executed in the last 2 seconds + # this allows to slide through a list of configurations + # without triggering refreshes of the scene in the middle of it + if time.time() - cached_scene["time"] > 2: + update_scene = True + else: + update_scene = False + + if update_scene: + scene = PlanningScene(robot) + try: + scene = robot.client.get_planning_scene() + except BackendFeatureNotSupportedError: + print("The selected backend does not support collision meshes. If you need collision mesh support, use a different backend.") + scene = None + show_cm = False + show_acm = False + + if update_scene and show_cm: + collision_meshes = [] + + for co in scene.world.collision_objects: + header = co.header + frame_id = header.frame_id + cms = co.to_collision_meshes() + + for cm in cms: + if cm.frame != Frame.worldXY(): + t = Transformation.from_frame(cm.frame) + mesh = cm.mesh.transformed(t) + else: + mesh = cm.mesh + + collision_meshes.extend(SceneObject(mesh).draw()) + + cached_scene["cm"] = collision_meshes + + collision_meshes = cached_scene.get("cm", []) + + if update_scene and show_acm: + attached_meshes = [] + + for aco in scene.robot_state.attached_collision_objects: + for acm in aco.to_attached_collision_meshes(): + frame_id = aco.object["header"]["frame_id"] + frame = robot.forward_kinematics(configuration, options=dict(link=frame_id)) + t = Transformation.from_frame(frame) + + # Local CM frame + if acm.collision_mesh.frame and acm.collision_mesh.frame != Frame.worldXY(): + t = t * Transformation.from_frame(acm.collision_mesh.frame) + + mesh = acm.collision_mesh.mesh.transformed(t) + + attached_meshes.extend(SceneObject(mesh).draw()) + + cached_scene["acm"] = attached_meshes + + attached_meshes = cached_scene.get("acm", []) + + cached_scene["time"] = time.time() + st[cached_scene_key] = cached_scene + + return ( + visual, + collision, + collision_meshes, + attached_meshes, + frames, + base_frame, + ee_frame, + ) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..d6178184f5f0f7e9aed4302a22c01e013eac3be8 GIT binary patch literal 1781 zcmbVNdu$VR9PiZCJ=|CrjtM552RJ5N@3HG`J=ble-J?p!(lxIBLE`o9*Y%{mJMM0@ z0|5taG6EZqjDgNz13pLunV1-&fE$~OF*9M|rSUO{AcjOoFutPB-*sIBiW0fxcfa5L zexKjx`~F^MLw#*d)|@Pp$&}-+b9v!hXp9Uqe2+)%zrZO|se4*8nX>bZk!E_S4^^tB3F?P7o67 z)~KLyO}&p_vy!(7Xk`Ub9%CUv1n3+Ri-aYOjoHxzFALYkHjW|*h`!Q}It+n`-_w9N zWfdTdg}``%ppa6Vg=A=&A(kK%K@m7%#YqYy8J3_~vJ^=^D3qoOLDuW4Ny>s>b~L2x z3X9{>Xw(udvB+vOPTFiXoS<-u!XN_ES|y!}VUjj~n!yD$UKJHxlqJMqg&T~>Xv z9G(tT!?el>MOr9iQIjYMa#Sn6=>l+ZIQPPwj%MhsJLVk@ zmB)>Oow5V}yNQXjn~HlH4m*aiOtmWbk!p58f*nqUt!$HY{85uBt=;W%`07T^_K;7W zs+u*jzhTR(dn7dX{*79E{qsmy?h5vp(4uL13;3dgmP@-ojyF~g=5*!$dQaQ>jbo#u zHx_=7J$d5F&RyOk?epTtzL=c+{&;WihlTm!UE^QR?Xz#_7+SaL%iRN8iJ_f^%p8R- zj<@n>kcacNg7=zEt?l`w>U?;!e@U9M?$fs73;PZ|G;!}`^m%@%3X4JD;3N;P46~8@u+dK)oQnP>Xv=v;uT8ch!qaich(e0o3 zLwvby<}2RIdxp=oKees)(9w*nt+QT#By+3dtr7E2Uk$Fk(3#tRU;(xL^x(mdHjqj| z-wSHNbF(|nM^?~cVfKR~rP0^M{~Wlu{M#RYZy(n7F=s2xMcs{a`#-y;pS(79bce(7 x^QEz>XS2)|=KjKPeb-@0%bWasw0r&BlP^BrGykz3=Ya8Ja#z>84ljLX<6raXVA}uy literal 0 HcmV?d00001 diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/metadata.json new file mode 100644 index 0000000000..dc15f9e870 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/metadata.json @@ -0,0 +1,93 @@ +{ + "name": "Robot Visualize", + "nickname": "Robot Visualize", + "category": "COMPAS FAB", + "subcategory": "Display", + "description": "Visualizes the robot.", + "exposure": 2, + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "group", + "description": "The planning group used for end-effector and base visualization.", + "typeHintID": "str" + }, + { + "name": "configuration", + "description": "The robot's full configuration." + }, + { + "name": "show_visual", + "description": "Whether or not to show the robot's visual meshes.", + "typeHintID": "bool" + }, + { + "name": "show_collision", + "description": "Whether or not to show the robot's collision meshes.", + "typeHintID": "bool" + }, + { + "name": "show_frames", + "description": "Whether or not to show the robot's joint frames.", + "typeHintID": "bool" + }, + { + "name": "show_base_frame", + "description": "Whether or not to show the robot's base frame.", + "typeHintID": "bool" + }, + { + "name": "show_end_effector_frame", + "description": "Whether or not to show the robot's end-effector frame.", + "typeHintID": "bool" + }, + { + "name": "show_cm", + "description": "Whether or not to show the collision meshes (if any).", + "typeHintID": "bool" + }, + { + "name": "show_acm", + "description": "Whether or not to show the attached collision meshes (if any).", + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "visual", + "description": "Rhino meshes of the robot's visual geometry (if any)." + }, + { + "name": "collision", + "description": "Rhino meshes of the robot's collision geometry (if any)." + }, + { + "name": "collision_meshes", + "description": "Rhino meshes of the scene's collision meshes (if any)." + }, + { + "name": "attached_meshes", + "description": "Rhino meshes of the scene's attached collision meshes (if any)." + }, + { + "name": "frames", + "description": "The robot's joint frames as Rhino planes (if any)." + }, + { + "name": "base_frame", + "description": "The robot's base frame as a Rhino plane (if any)." + }, + { + "name": "ee_frame", + "description": "The robot's end-effector frame as a Rhino plane (if any)." + } + ] + } +} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py new file mode 100644 index 0000000000..cd923ec502 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py @@ -0,0 +1,46 @@ +# r: compas_fab>=1.0.2 +""" +Visualizes a trajectory. + +COMPAS FAB v1.0.2 +""" + +import Grasshopper + +from compas_ghpython.drawing import draw_frame +from compas_ghpython.sets import list_to_ghtree + + +class TrajectoryVisualize(Grasshopper.Kernel.GH_ScriptInstance): + def RunScript(self, robot, group, trajectory): + start_configuration = None + configurations = [] + fraction = 0.0 + time = 0.0 + + planes = [] + positions = [] + velocities = [] + accelerations = [] + + if robot and trajectory: + group = group or robot.main_group_name + + for c in trajectory.points: + configurations.append(robot.merge_group_with_full_configuration(c, trajectory.start_configuration, group)) + frame = robot.forward_kinematics(c, group, options=dict(solver="model")) + planes.append(draw_frame(frame)) + positions.append(c.positions) + velocities.append(c.velocities) + accelerations.append(c.accelerations) + + start_configuration = trajectory.start_configuration + fraction = trajectory.fraction + time = trajectory.time_from_start + + P = list_to_ghtree(list(zip(*positions))) + V = list_to_ghtree(list(zip(*velocities))) + A = list_to_ghtree(list(zip(*accelerations))) + + # return outputs if you have them; here I try it for you: + return (start_configuration, configurations, fraction, time, planes, P, V, A) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..85c6c895a5dafd8949c90e761f736c843d74a163 GIT binary patch literal 1798 zcmbVNZEO=|9KWt?gVnJia|%OtIS>ZyURS!_Wn~?0*@_gKZNQdHvg_T`^;p|G?ryYO zFu;b*whw}QFiMQ>gUZmTn+hzTVc0odzG#r3evuN1FCoPFl87HIlNg$Y=$#I%&Jc*U`246# zRsm`wC|sZ@2CZfZ+Ge%ds7jQf7>cAek~D+UHjc7#bTyiJFeptGgIuF~OGXy_a$q4{ zS2&W4#bQLPijdU~l4e$Oq&u-GM<( zXH1AFS=y+UaT9bH8Rr#}CMd(CG*A?>IHgMsr-h3G3Bn)(qPhmLbQY_GWL?%m^1o1L zkM9!zUF-8@Wz4HfB$5@O>7ISijEqC(MQhF73LqPSCU>a<@a%(Xt}?7qIF|}|T~?cA zIXoLE|E$U=LlX>Iu}g}Ia!gw{=K^r^I&fexYBWwUIJL2vX1FRF$FLhHnxm*0s81H9 zVE4RGJ4aRD2Zbpk@;d)OuqbdrS&i_}U@5|P08)u|U})BjoJ$VNDijR0tD3v-b-Dbi z9F)TFKx?eujCwsT8_n8mI785Bb$ve08`X3^DgdwBfkEdGk|c6Muu2S6!+a8hb}Jqb ztyMT%#R|Bc6-1F{fyf7fv-jPy&}F2+?0xZn-hW1wUw4H@hYh6fZlpw-jPsw z(kR#&JIH^Um_D0R+_P}lF^pxlRl$#Jvjb7s;Z)elZc(1C2x5B4>#l2VxG3D6o~||H zbaFVCs{Asx8tE@`?ll)%ia$bzho5}=LgL`r1?asaiK5ggWH?oJEKr8tE1bA#zqWfk zU|Lv~V%k>r|KNP=uPuS;&El%Q-zO@wv%7S;zB^y%P-tzA%9Xj{u`af2(&!>(a zI8t4n(w?$@bNajfA9G&by|uF9;Mrem7Qa5dX0q2j#yoy0=j8P}&SxK?J}zLFj4XWm z#^J&9$vq}(z0EnAQ)+Rp`>FQCYsqrG?~UVUzIA;tW=`e|P3E>-9W|Za|LN@&A0n4W z{71)>{lC?&riBuVGwsv4 zj&^?Z*5Wg7mVcd7%AcGrwsf~|s<;?55#{0W9Xl7TJ$2(y?m&BJnL1U$`#ckqromud z;@rrk#MO?eo%X|xOG+(QUTk||NlnRDp`mNN!9%I3cYZgm>>0m({m*2l@u%{7w!7ck H+}`^)L=#;P literal 0 HcmV?d00001 diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/metadata.json b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/metadata.json new file mode 100644 index 0000000000..4bd39bd362 --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/metadata.json @@ -0,0 +1,63 @@ +{ + "name": "Trajectory Visualize", + "nickname": "Trajectory Visualize", + "category": "COMPAS FAB", + "subcategory": "Display", + "description": "Visualizes the trajectory.", + "exposure": 2, + + "ghpython": { + "isAdvancedMode": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "robot", + "description": "The robot.", + "wireDisplay": "hidden" + }, + { + "name": "group", + "description": "The planning group for which this trajectory was planned.", + "typeHintID": "str" + }, + { + "name": "trajectory", + "description": "The calculated trajectory." + } + ], + "outputParameters": [ + { + "name": "start_configuration", + "description": "The start configuration of the trajectory." + }, + { + "name": "configurations", + "description": "The full configurations along the trajectory." + }, + { + "name": "fraction", + "description": "Indicates the fraction of requested trajectory that was calculated, e.g. 1 means the full trajectory was found." + }, + { + "name": "time", + "description": "The time which is needed to execute that trajectory at full speed." + }, + { + "name": "planes", + "description": "The planes of the robot's end-effector." + }, + { + "name": "P", + "description": "The positions along the trajectory" + }, + { + "name": "V", + "description": "The velocities along the trajectory" + }, + { + "name": "A", + "description": "The accelerations along the trajectory" + } + ] + } +} From 40412958a6a0b52e4f874bcf4a0ed388664616a5 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 14:38:13 +0100 Subject: [PATCH 03/36] fixed plan cartesian motion component --- .../components_cpython/Cf_PlanCartesianMotion/code.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py index 697a139c44..72bd11dc67 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py @@ -6,13 +6,13 @@ """ import System + import Grasshopper from compas_rhino.conversions import plane_to_compas_frame from scriptcontext import sticky as st from compas_fab.ghpython.components import create_id -from compas_fab.robots import FrameWaypoints class PlanCartesianMotion(Grasshopper.Kernel.GH_ScriptInstance): @@ -27,7 +27,7 @@ def RunScript( max_step: float, compute: bool, ): - key = create_id(self, "trajectory") + key = create_id(ghenv.Component, "trajectory") # noqa: F821 max_step = float(max_step) if max_step else 0.01 path_constraints = list(path_constraints) if path_constraints else None @@ -35,9 +35,8 @@ def RunScript( if robot and robot.client and robot.client.is_connected and start_configuration and planes and compute: frames = [plane_to_compas_frame(plane) for plane in planes] - frame_waypoints = FrameWaypoints(frames) st[key] = robot.plan_cartesian_motion( - frame_waypoints, + frames, start_configuration=start_configuration, group=group, options=dict( From ae2ca086526484c2a5914baf13befee3f207d365 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 14:39:33 +0100 Subject: [PATCH 04/36] fixed plan motion component --- .../components_cpython/Cf_PlanMotion/code.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py index f7717c31f2..42fd7c5791 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py @@ -17,15 +17,15 @@ class PlanMotion(Grasshopper.Kernel.GH_ScriptInstance): def RunScript( self, robot, - goal_constraints, + goal_constraints: System.Collections.Generic.List[object], start_configuration, - group, - attached_collision_meshes, - path_constraints, - planner_id, - compute, + group: str, + attached_collision_meshes: System.Collections.Generic.List[object], + path_constraints: System.Collections.Generic.List[object], + planner_id: str, + compute: bool, ): - key = create_id(self, "trajectory") + key = create_id(ghenv.Component, "trajectory") # noqa: F821 path_constraints = list(path_constraints) if path_constraints else None attached_collision_meshes = list(attached_collision_meshes) if attached_collision_meshes else None From eed9fcbe8b7666b5b093422b83578c7109a3e4ee Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 14:44:04 +0100 Subject: [PATCH 05/36] removed components from an alternative future --- .../Cf_FrameTargetFromPlane/code.py | 44 ---------------- .../Cf_FrameTargetFromPlane/icon.png | Bin 620 -> 0 bytes .../Cf_FrameTargetFromPlane/metadata.json | 48 ------------------ .../Cf_PointAxisTarget/code.py | 28 ---------- .../Cf_PointAxisTarget/icon.png | Bin 620 -> 0 bytes .../Cf_PointAxisTarget/metadata.json | 37 -------------- 6 files changed, 157 deletions(-) delete mode 100644 src/compas_fab/ghpython/components_cpython/Cf_FrameTargetFromPlane/code.py delete mode 100644 src/compas_fab/ghpython/components_cpython/Cf_FrameTargetFromPlane/icon.png delete mode 100644 src/compas_fab/ghpython/components_cpython/Cf_FrameTargetFromPlane/metadata.json delete mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PointAxisTarget/code.py delete mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PointAxisTarget/icon.png delete mode 100644 src/compas_fab/ghpython/components_cpython/Cf_PointAxisTarget/metadata.json diff --git a/src/compas_fab/ghpython/components_cpython/Cf_FrameTargetFromPlane/code.py b/src/compas_fab/ghpython/components_cpython/Cf_FrameTargetFromPlane/code.py deleted file mode 100644 index 5775d10f8c..0000000000 --- a/src/compas_fab/ghpython/components_cpython/Cf_FrameTargetFromPlane/code.py +++ /dev/null @@ -1,44 +0,0 @@ -""" -Create a fully constrained pose target for the robot's end-effector using a GH Plane or compas Frame. - -COMPAS FAB v1.0.2 -""" - -import math - -from compas_rhino.conversions import plane_to_compas_frame -from ghpythonlib.componentbase import executingcomponent as component - -from compas.geometry import Frame -from compas_fab.robots import FrameTarget - - -class FrameTargetFromPlaneComponent(component): - def RunScript( - self, plane, tolerance_position, tolerance_xaxis, tolerance_yaxis, tolerance_zaxis, tool_coordinate_frame - ): - target = None - if plane: - # Convert Rhino geometry to COMPAS geometry - frame = plane - if not isinstance(frame, Frame): - frame = plane_to_compas_frame(frame) - if not isinstance(tool_coordinate_frame, Frame): - tool_coordinate_frame = plane_to_compas_frame(tool_coordinate_frame) - - # Tolerance values - DEFAULT_TOLERANCE_METERS = 0.001 - DEFAULT_TOLERANCE_RADIANS = math.radians(1) - tolerance_position = tolerance_position or DEFAULT_TOLERANCE_METERS - tolerance_xaxis = tolerance_xaxis or DEFAULT_TOLERANCE_RADIANS - tolerance_yaxis = tolerance_yaxis or DEFAULT_TOLERANCE_RADIANS - tolerance_zaxis = tolerance_zaxis or DEFAULT_TOLERANCE_RADIANS - tolerance_orientation = [ - (tolerance_xaxis), - (tolerance_yaxis), - (tolerance_zaxis), - ] - - target = FrameTarget(frame, tolerance_position, tolerance_orientation, tool_coordinate_frame) - - return target diff --git a/src/compas_fab/ghpython/components_cpython/Cf_FrameTargetFromPlane/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_FrameTargetFromPlane/icon.png deleted file mode 100644 index 0613d36bb28ba43396ababe26de3bb32bbe036c7..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 620 zcmV-y0+aoTP)2 zFCMhL^w{+x2tDLnMC+wz^`^OO4*~5d^j289Lwl?QKT;1iX3AS8vW9HjEat(m`*z>F z-~0N_kWvc0l)fmwiArN}W4IT9s3ZW`Jp%xc*Q;j$ivVWgmMLlmh?_-60HV??fV5T@ z9~_`1>W9yuC2B)YRCmHe>$HF)8#r?AvkP01h!!T6FbkG2omeF*6ZthC!QWIeaW%~E zv_*N^qTsl4$#R_l+MY#aq5^;&0Gr!WbF15#mnp2+y8z|^R7G+6Q=*o5!~D=&&H?N( z<6Y^=jsOAYP1$|*5#Oq5Z1BiV9Aq&1eFPxAxyzhm!{~X-h7vh{F*$1%1sjFpm#t~)?$4~2{H`k{N zRoo0Vo?Oh<3eEkl^c;OC&RD{{9dExxWg`Btrt9cDSE>?>!V+fl!21B;r2e>me4Y6x z`Y-#RJ^C{+31D5DrU1ML@Rj|z(z7F7IF>MN?Yt{-rAJ+nCTobZtRb9$Zf6al0(zeH ztRdE#-?}9lH!#Np3;AEv&%HZ6h$@yanPl7_L)V@4RXssP&5O6L^op?nbU&EAekLuy zc=DKNx6We$1c3OiU}p^EkFzU19_PCf_mm7Y)ef~$3jYB=lVSJb+G6kk0000=1.0.2 -""" -Create a point and axis target for the robot's end-effector motion planning. - -COMPAS FAB v1.0.2 -""" - -import Grasshopper - -from compas_rhino.conversions import point_to_compas -from compas_rhino.conversions import vector_to_compas - -from compas.geometry import Point -from compas.geometry import Vector -from compas_fab.robots import PointAxisTarget - - -class PointAxisTargetComponent(Grasshopper.Kernel.GH_ScriptInstance): - def RunScript(self, point, target_z_vector, tolerance_position, tool_coordinate_frame): - target = None - if point: - # Convert Rhino geometry to COMPAS geometry - point = point if isinstance(point, Point) else point_to_compas(point) - target_z_vector = target_z_vector if isinstance(target_z_vector, Vector) else vector_to_compas(target_z_vector) - - target = PointAxisTarget(point, target_z_vector, tolerance_position, tool_coordinate_frame) - - return target diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PointAxisTarget/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_PointAxisTarget/icon.png deleted file mode 100644 index 0613d36bb28ba43396ababe26de3bb32bbe036c7..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 620 zcmV-y0+aoTP)2 zFCMhL^w{+x2tDLnMC+wz^`^OO4*~5d^j289Lwl?QKT;1iX3AS8vW9HjEat(m`*z>F z-~0N_kWvc0l)fmwiArN}W4IT9s3ZW`Jp%xc*Q;j$ivVWgmMLlmh?_-60HV??fV5T@ z9~_`1>W9yuC2B)YRCmHe>$HF)8#r?AvkP01h!!T6FbkG2omeF*6ZthC!QWIeaW%~E zv_*N^qTsl4$#R_l+MY#aq5^;&0Gr!WbF15#mnp2+y8z|^R7G+6Q=*o5!~D=&&H?N( z<6Y^=jsOAYP1$|*5#Oq5Z1BiV9Aq&1eFPxAxyzhm!{~X-h7vh{F*$1%1sjFpm#t~)?$4~2{H`k{N zRoo0Vo?Oh<3eEkl^c;OC&RD{{9dExxWg`Btrt9cDSE>?>!V+fl!21B;r2e>me4Y6x z`Y-#RJ^C{+31D5DrU1ML@Rj|z(z7F7IF>MN?Yt{-rAJ+nCTobZtRb9$Zf6al0(zeH ztRdE#-?}9lH!#Np3;AEv&%HZ6h$@yanPl7_L)V@4RXssP&5O6L^op?nbU&EAekLuy zc=DKNx6We$1c3OiU}p^EkFzU19_PCf_mm7Y)ef~$3jYB=lVSJb+G6kk0000 Date: Mon, 24 Mar 2025 14:44:26 +0100 Subject: [PATCH 06/36] re-introduced current timeline components --- .../Cf_ConstraintsFromConfiguration/code.py | 31 ++++++++++ .../Cf_ConstraintsFromConfiguration/icon.png | Bin 0 -> 680 bytes .../metadata.json | 45 ++++++++++++++ .../Cf_ConstraintsFromPlane/code.py | 30 ++++++++++ .../Cf_ConstraintsFromPlane/icon.png | Bin 0 -> 620 bytes .../Cf_ConstraintsFromPlane/metadata.json | 55 ++++++++++++++++++ 6 files changed, 161 insertions(+) create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/metadata.json create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/code.py create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/icon.png create mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/metadata.json diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py new file mode 100644 index 0000000000..9c31653a3d --- /dev/null +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py @@ -0,0 +1,31 @@ +""" +Create joint constraints for each of the robot's configurable joints based on a given target configuration. + +COMPAS FAB v1.0.2 +""" + +import math + +from ghpythonlib.componentbase import executingcomponent as component + + +class ConstraintsFromTargetConfiguration(component): + DEFAULT_TOLERANCE_METERS = 0.001 + DEFAULT_TOLERANCE_RADIANS = math.radians(1) + + def RunScript(self, robot, target_configuration, tolerance_above, tolerance_below, group): + if robot and target_configuration: + tolerance_above = tolerance_above or self._generate_default_tolerances(robot.get_configurable_joints(group)) + tolerance_below = tolerance_below or self._generate_default_tolerances(robot.get_configurable_joints(group)) + + constraints = robot.constraints_from_configuration( + configuration=target_configuration, + tolerances_above=tolerance_above, + tolerances_below=tolerance_below, + group=group, + ) + + return constraints + + def _generate_default_tolerances(self, joints): + return [self.DEFAULT_TOLERANCE_METERS if j.is_scalable() else self.DEFAULT_TOLERANCE_RADIANS for j in joints] diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..d721d6bc2efe3fc764c59075c3fe3c2bfebf5194 GIT binary patch literal 680 zcmV;Z0$2TsP);UVI~yF^RhiJ{Xdjym{Yy-+ON+ z8)aGIe@bs1+m1>Gj!Nz>qq`OWfC+$8Moz){HsfL9Isj*3{0h$@bzm9D<=KH%pEe%=P~y=7nJ zZ4t{$j;K}sP{iKkmf}4Cj|>O)`TaeBkSU@F&Wc!uO(1p-b^tu%=^Fr7idfE!?}CjI zm7h5mbBdyfrR`iJz?eOQ!K+{@Og#dLxDaOOkiQzuQAy68X*n!n8Fd-ylBPbeleXX1 zRDtkDf$&aN0w`ih8{S&Wla~FIlie9TWD**ZyrFmA%SL0E^ z74g!Jf`9|zQ4hT4wK+d2>ZerZf$(Z{nvJGCKLc>10oUF9_^yiT=hhB>+CK-p(`IV%3cEY*ZHlyfUHge{(2GYCuRE>>q zUr^rKLVt0L-udD&fIIk|6MZON({vJ*X`W4&x^URQ!9Nx2GEeg?1OY5Nd{2 zFCMhL^w{+x2tDLnMC+wz^`^OO4*~5d^j289Lwl?QKT;1iX3AS8vW9HjEat(m`*z>F z-~0N_kWvc0l)fmwiArN}W4IT9s3ZW`Jp%xc*Q;j$ivVWgmMLlmh?_-60HV??fV5T@ z9~_`1>W9yuC2B)YRCmHe>$HF)8#r?AvkP01h!!T6FbkG2omeF*6ZthC!QWIeaW%~E zv_*N^qTsl4$#R_l+MY#aq5^;&0Gr!WbF15#mnp2+y8z|^R7G+6Q=*o5!~D=&&H?N( z<6Y^=jsOAYP1$|*5#Oq5Z1BiV9Aq&1eFPxAxyzhm!{~X-h7vh{F*$1%1sjFpm#t~)?$4~2{H`k{N zRoo0Vo?Oh<3eEkl^c;OC&RD{{9dExxWg`Btrt9cDSE>?>!V+fl!21B;r2e>me4Y6x z`Y-#RJ^C{+31D5DrU1ML@Rj|z(z7F7IF>MN?Yt{-rAJ+nCTobZtRb9$Zf6al0(zeH ztRdE#-?}9lH!#Np3;AEv&%HZ6h$@yanPl7_L)V@4RXssP&5O6L^op?nbU&EAekLuy zc=DKNx6We$1c3OiU}p^EkFzU19_PCf_mm7Y)ef~$3jYB=lVSJb+G6kk0000 Date: Mon, 24 Mar 2025 14:47:28 +0100 Subject: [PATCH 07/36] cpythonized remaining components --- .../Cf_ConstraintsFromConfiguration/code.py | 7 +++++-- .../components_cpython/Cf_ConstraintsFromPlane/code.py | 8 ++++++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py index 9c31653a3d..91591318c0 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py @@ -1,3 +1,4 @@ +# r: compas_fab>=1.0.2 """ Create joint constraints for each of the robot's configurable joints based on a given target configuration. @@ -6,10 +7,12 @@ import math -from ghpythonlib.componentbase import executingcomponent as component +import System +import Grasshopper -class ConstraintsFromTargetConfiguration(component): + +class ConstraintsFromTargetConfiguration(Grasshopper.Kernel.GH_ScriptInstance): DEFAULT_TOLERANCE_METERS = 0.001 DEFAULT_TOLERANCE_RADIANS = math.radians(1) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/code.py index 4c58d804fb..cf978fca81 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/code.py @@ -1,3 +1,4 @@ +# r: compas_fab>=1.0.2 """ Create a position and an orientation constraint from a plane calculated for the group's end-effector link. @@ -6,11 +7,14 @@ import math +import System + +import Grasshopper + from compas_rhino.conversions import plane_to_compas_frame -from ghpythonlib.componentbase import executingcomponent as component -class ConstraintsFromPlane(component): +class ConstraintsFromPlane(Grasshopper.Kernel.GH_ScriptInstance): def RunScript(self, robot, plane, group, tolerance_position, tolerance_xaxis, tolerance_yaxis, tolerance_zaxis): goal_constraints = None if robot and plane: From 27bf75c1f66167a010d2c85d78dcfb01b39122e4 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 14:53:14 +0100 Subject: [PATCH 08/36] fixed calls to create_id --- .../ghpython/components_cpython/Cf_PlanningScene/code.py | 2 +- .../ghpython/components_cpython/Cf_RosConnect/code.py | 2 +- .../ghpython/components_cpython/Cf_RosRobot/code.py | 2 +- .../ghpython/components_cpython/Cf_RosTopicPublish/code.py | 2 +- .../ghpython/components_cpython/Cf_RosTopicSubscribe/code.py | 4 ++-- .../ghpython/components_cpython/Cf_VisualizeRobot/code.py | 2 +- 6 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py index 171b005668..d1131ce319 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py @@ -15,7 +15,7 @@ class PlanningSceneComponent(Grasshopper.Kernel.GH_ScriptInstance): def RunScript(self, robot): - key = create_id(self, "planning_scene") + key = create_id(ghenv.Component, "planning_scene") # noqa: F821 if robot: st[key] = PlanningScene(robot) return st.get(key, None) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py index 02a225b755..6ded731250 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py @@ -20,7 +20,7 @@ def RunScript(self, ip, port, connect): ip = ip or "127.0.0.1" port = port or 9090 - key = create_id(self, "ros_client") + key = create_id(ghenv.Component, "ros_client") # noqa: F821 ros_client = st.get(key, None) if ros_client: diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py index b25de8827a..d2df6928d4 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py @@ -15,7 +15,7 @@ class ROSRobot(Grasshopper.Kernel.GH_ScriptInstance): def RunScript(self, ros_client, load): - key = create_id(self, "robot") + key = create_id(ghenv.Component, "robot") # noqa: F821 if ros_client and ros_client.is_connected and load: # Load URDF from ROS diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py index f77e50378a..0d4d3725a5 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py @@ -23,7 +23,7 @@ def RunScript(self, ros_client, topic_name, topic_type, msg): if not topic_type: raise ValueError("Please specify the type of the topic") - key = create_id(self, "topic") + key = create_id(ghenv.Component, "topic") # noqa: F821 topic = st.get(key, None) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py index 3e83b13403..6c883940a1 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py @@ -30,8 +30,8 @@ def RunScript(self, ros_client, topic_name, topic_type, interval, start, stop): self.is_updating = False self.is_subscribed = False - self.msg_key = create_id(self, "last_msg") - key = create_id(self, "topic") + self.msg_key = create_id(ghenv.Component, "last_msg") # noqa: F821 + key = create_id(ghenv.Component, "topic") # noqa: F821 last_msg = st.get(self.msg_key, None) topic = st.get(key, None) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py index 5983fcbb06..9d6a290ed7 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py @@ -75,7 +75,7 @@ def RunScript( frame = frame_to_rhino_plane(compas_frame) frames.append(frame) - cached_scene_key = create_id(self, "cached_scene") + cached_scene_key = create_id(ghenv.Component, "cached_scene") # noqa: F821 if show_cm or show_acm: cached_scene = st.get(cached_scene_key) From c861d6562faba68dfebad4b8e555ca2e383fad0f Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 14:59:13 +0100 Subject: [PATCH 09/36] argname and typing in collision mesh component --- .../components_cpython/Cf_CollisionMesh/code.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py index bc4f0fb75d..f4ba268b90 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py @@ -6,6 +6,7 @@ """ import Grasshopper +import Rhino from compas_rhino.conversions import mesh_to_compas @@ -13,7 +14,7 @@ class CollisionMeshComponent(Grasshopper.Kernel.GH_ScriptInstance): - def RunScript(self, scene, M, name, add, append, remove): + def RunScript(self, scene, mesh: Rhino.Geometry.Mesh, identifier: str, add: bool, append: bool, remove: bool): ok = False self.Message = "" @@ -21,9 +22,9 @@ def RunScript(self, scene, M, name, add, append, remove): self.Message = "Use only one operation at a time\n(add, append or remove)" raise Exception(self.Message) - if scene and M and name: - mesh = mesh_to_compas(M) - collision_mesh = CollisionMesh(mesh, name) + if scene and mesh and identifier: + compas_mesh = mesh_to_compas(mesh) + collision_mesh = CollisionMesh(compas_mesh, identifier) if add: scene.add_collision_mesh(collision_mesh) self.Message = "Added" @@ -33,7 +34,7 @@ def RunScript(self, scene, M, name, add, append, remove): self.Message = "Appended" ok = True if remove: - scene.remove_collision_mesh(name) + scene.remove_collision_mesh(identifier) self.Message = "Removed" ok = True return ok From 10e8a9ee5738245eb6ffe33f974ad157a2cd4ee9 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 15:03:43 +0100 Subject: [PATCH 10/36] some more typing fixes --- .../ghpython/components_cpython/Cf_AttachTool/code.py | 3 ++- .../components_cpython/Cf_AttachedCollisionMesh/code.py | 5 ++++- .../ghpython/components_cpython/Cf_RosTopicSubscribe/code.py | 4 ++-- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py index 61568d71c3..103ab074ee 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py @@ -6,6 +6,7 @@ """ import Grasshopper +import Rhino from compas.geometry import Frame from compas_rhino.conversions import mesh_to_compas @@ -15,7 +16,7 @@ class AttachToolComponent(Grasshopper.Kernel.GH_ScriptInstance): - def RunScript(self, robot, visual_mesh, collision_mesh, tcf_plane, group, connected_to): + def RunScript(self, robot, visual_mesh: Rhino.Geometry.Mesh, collision_mesh: Rhino.Geometry.Mesh, tcf_plane, group, connected_to): if robot and robot.client and robot.client.is_connected and visual_mesh: if not collision_mesh: collision_mesh = visual_mesh diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py index c2144a4555..9cb0401fee 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py @@ -5,7 +5,10 @@ COMPAS FAB v1.0.2 """ +import System + import Grasshopper +import Rhino from compas_rhino.conversions import mesh_to_compas @@ -14,7 +17,7 @@ class AttachedCollisionMeshComponent(Grasshopper.Kernel.GH_ScriptInstance): - def RunScript(self, scene, mesh, identifier, link_name, touch_links, add, remove): + def RunScript(self, scene, mesh: Rhino.Geometry.Mesh, identifier: str, link_name: str, touch_links: System.Collections.Generic.List[str], add: bool, remove: bool): attached_collision_mesh = None if scene and mesh and identifier and link_name: compas_mesh = mesh_to_compas(mesh) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py index 6c883940a1..1f589e328c 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py @@ -8,7 +8,7 @@ import time import Grasshopper -import Grasshopper.Kernel + from roslibpy import Topic from scriptcontext import sticky as st @@ -17,7 +17,7 @@ class ROSTopicSubscribe(Grasshopper.Kernel.GH_ScriptInstance): - def RunScript(self, ros_client, topic_name, topic_type, interval, start, stop): + def RunScript(self, ros_client, topic_name: str, topic_type: str, interval: int, start: bool, stop: bool): if not topic_name: raise ValueError("Please specify the name of the topic") if not topic_type: From 010cca2ffa13876bf59d85e7081d190d52176ae7 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 15:12:57 +0100 Subject: [PATCH 11/36] removed another component that snuck in from the future --- .../Cf_ConfigurationTarget/code.py | 32 -------------- .../Cf_ConfigurationTarget/icon.png | Bin 680 -> 0 bytes .../Cf_ConfigurationTarget/metadata.json | 39 ------------------ 3 files changed, 71 deletions(-) delete mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/code.py delete mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/icon.png delete mode 100644 src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/metadata.json diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/code.py deleted file mode 100644 index 2318b67ac8..0000000000 --- a/src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/code.py +++ /dev/null @@ -1,32 +0,0 @@ -# r: compas_fab>=1.0.2 -""" -Create configuration target for the robot's end-effector motion planning. - -COMPAS FAB v1.0.2 -""" - -import Grasshopper - -import math - - -from compas_fab.robots import ConfigurationTarget - - -class ConfigurationTargetComponent(Grasshopper.Kernel.GH_ScriptInstance): - DEFAULT_TOLERANCE_METERS = 0.001 - DEFAULT_TOLERANCE_RADIANS = math.radians(1) - - def RunScript(self, robot, target_configuration, tolerances_above, tolerances_below): - if robot and target_configuration: - default_tolerances_above, default_tolerances_below = ConfigurationTarget.generate_default_tolerances( - target_configuration, self.DEFAULT_TOLERANCE_METERS, self.DEFAULT_TOLERANCE_RADIANS - ) - - target = ConfigurationTarget( - target_configuration=target_configuration, - tolerances_above=tolerances_above or default_tolerances_above, - tolerances_below=tolerances_below or default_tolerances_below, - ) - - return target diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/icon.png b/src/compas_fab/ghpython/components_cpython/Cf_ConfigurationTarget/icon.png deleted file mode 100644 index d721d6bc2efe3fc764c59075c3fe3c2bfebf5194..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 680 zcmV;Z0$2TsP);UVI~yF^RhiJ{Xdjym{Yy-+ON+ z8)aGIe@bs1+m1>Gj!Nz>qq`OWfC+$8Moz){HsfL9Isj*3{0h$@bzm9D<=KH%pEe%=P~y=7nJ zZ4t{$j;K}sP{iKkmf}4Cj|>O)`TaeBkSU@F&Wc!uO(1p-b^tu%=^Fr7idfE!?}CjI zm7h5mbBdyfrR`iJz?eOQ!K+{@Og#dLxDaOOkiQzuQAy68X*n!n8Fd-ylBPbeleXX1 zRDtkDf$&aN0w`ih8{S&Wla~FIlie9TWD**ZyrFmA%SL0E^ z74g!Jf`9|zQ4hT4wK+d2>ZerZf$(Z{nvJGCKLc>10oUF9_^yiT=hhB>+CK-p(`IV%3cEY*ZHlyfUHge{(2GYCuRE>>q zUr^rKLVt0L-udD&fIIk|6MZON({vJ*X`W4&x^URQ!9Nx2GEeg?1OY5Nd{ Date: Mon, 24 Mar 2025 16:01:15 +0100 Subject: [PATCH 12/36] added helper functions for GH message printing --- .../ghpython/components/__init__.py | 57 +++++++++++++++++++ 1 file changed, 57 insertions(+) diff --git a/src/compas_fab/ghpython/components/__init__.py b/src/compas_fab/ghpython/components/__init__.py index cecffffbf4..839ed05b2f 100644 --- a/src/compas_fab/ghpython/components/__init__.py +++ b/src/compas_fab/ghpython/components/__init__.py @@ -1,5 +1,62 @@ from __future__ import absolute_import +try: + import Grasshopper # type: ignore +except ImportError: + pass + def create_id(component, name): return "{}_{}".format(name, component.InstanceGuid) + + +def warning(component, message): + """Add a warning message to the component. + + Parameters + ---------- + component : Grasshopper.Kernel.IGH_Component + The component instance. Pre-Rhino8 use `self`. Post-Rhino8 use `ghenv.Component`. + message : str + The message to display. + """ + component.AddRuntimeMessage(Grasshopper.Kernel.GH_RuntimeMessageLevel.Warning, message) + + +def error(component, message): + """Add an error message to the component. + + Parameters + ---------- + component : Grasshopper.Kernel.IGH_Component + The component instance. Pre-Rhino8 use `self`. Post-Rhino8 use `ghenv.Component`. + message : str + The message to display. + """ + component.AddRuntimeMessage(Grasshopper.Kernel.GH_RuntimeMessageLevel.Error, message) + + +def remark(component, message): + """Add a remark message to the component. + + Parameters + ---------- + component : Grasshopper.Kernel.IGH_Component + The component instance. Pre-Rhino8 use `self`. Post-Rhino8 use `ghenv.Component`. + message : str + The message to display. + """ + component.AddRuntimeMessage(Grasshopper.Kernel.GH_RuntimeMessageLevel.Remark, message) + + +def message(component, message): + """Add a text that will appear under the component. + + Parameters + ---------- + component : Grasshopper.Kernel.IGH_Component + The component instance. Pre-Rhino8 use `self`. Post-Rhino8 use `ghenv.Component`. + message : str + The message to display. + """ + component.Message = message From 342728d98266a8f1558354b726e34e156a2004ce Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 16:01:29 +0100 Subject: [PATCH 13/36] updated components with new message helpers --- .../Cf_CollisionMesh/code.py | 18 ++++++++++++------ .../Cf_RosTopicPublish/code.py | 14 +++++++++----- .../Cf_RosTopicSubscribe/code.py | 5 +++-- 3 files changed, 24 insertions(+), 13 deletions(-) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py index f4ba268b90..1f1031922e 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py @@ -11,30 +11,36 @@ from compas_rhino.conversions import mesh_to_compas from compas_fab.robots import CollisionMesh +from compas_fab.ghpython.components import message +from compas_fab.ghpython.components import error class CollisionMeshComponent(Grasshopper.Kernel.GH_ScriptInstance): + @property + def component(self): + return ghenv.Component + def RunScript(self, scene, mesh: Rhino.Geometry.Mesh, identifier: str, add: bool, append: bool, remove: bool): ok = False - self.Message = "" + message(self.component, "") if (add and append) or (append and remove) or (add and remove): - self.Message = "Use only one operation at a time\n(add, append or remove)" - raise Exception(self.Message) + error(self.component, "Use only one operation at a time\n(add, append or remove)") + return if scene and mesh and identifier: compas_mesh = mesh_to_compas(mesh) collision_mesh = CollisionMesh(compas_mesh, identifier) if add: scene.add_collision_mesh(collision_mesh) - self.Message = "Added" + message(self.component, "Added") ok = True if append: scene.append_collision_mesh(collision_mesh) - self.Message = "Appended" + message(self.component, "Appended") ok = True if remove: scene.remove_collision_mesh(identifier) - self.Message = "Removed" + message(self.component, "Removed") ok = True return ok diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py index 0d4d3725a5..ea3b402e26 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py @@ -14,14 +14,18 @@ from compas_fab.backends.ros.messages import ROSmsg from compas_fab.ghpython.components import create_id +from compas_fab.ghpython.components import warning +from compas_fab.ghpython.components import message class ROSTopicPublish(Grasshopper.Kernel.GH_ScriptInstance): - def RunScript(self, ros_client, topic_name, topic_type, msg): + def RunScript(self, ros_client, topic_name: str, topic_type: str, msg): if not topic_name: - raise ValueError("Please specify the name of the topic") + warning(ghenv.Component, "Please specify the name of the topic") + return if not topic_type: - raise ValueError("Please specify the type of the topic") + warning(ghenv.Component, "Please specify the type of the topic") + return key = create_id(ghenv.Component, "topic") # noqa: F821 @@ -40,9 +44,9 @@ def RunScript(self, ros_client, topic_name, topic_type, msg): if msg: msg = ROSmsg.parse(msg, topic_type) topic.publish(msg.msg) - self.Message = "Message published" + message(ghenv.Component, "Message published") else: if self.is_advertised: - self.Message = "Topic advertised" + message(ghenv.Component, "Topic advertised") return (topic, self.is_advertised) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py index 1f589e328c..7cca6727e7 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py @@ -14,6 +14,7 @@ from compas_fab.backends.ros.messages import ROSmsg from compas_fab.ghpython.components import create_id +from compas_fab.ghpython.components import message class ROSTopicSubscribe(Grasshopper.Kernel.GH_ScriptInstance): @@ -55,9 +56,9 @@ def RunScript(self, ros_client, topic_name: str, topic_type: str, interval: int, last_msg = ROSmsg.parse(last_msg, topic_type) if self.is_subscribed: - self.Message = "Subscribed, {} messages".format(self.message_count) + message(ghenv.Component, f"Subscribed, {self.message_count} messages") # noqa: F821 else: - self.Message = "Not subscribed" + message(ghenv.Component, "Not subscribed") # noqa: F821 return (topic, last_msg, self.is_subscribed) From 3c7003cbaa5f2c55242854af02167743aa891487 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 16:11:22 +0100 Subject: [PATCH 14/36] function wants Sphere in primitive form --- .../backends/kinematics/solvers/spherical_wrist.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/compas_fab/backends/kinematics/solvers/spherical_wrist.py b/src/compas_fab/backends/kinematics/solvers/spherical_wrist.py index 9025cb03d1..a555d24116 100644 --- a/src/compas_fab/backends/kinematics/solvers/spherical_wrist.py +++ b/src/compas_fab/backends/kinematics/solvers/spherical_wrist.py @@ -174,7 +174,10 @@ def inverse_kinematics_spherical_wrist(target_frame, points): elbow_frame = Frame(p1A, elbow_dir, [0, 0, 1]) elbow_plane = (p1A, elbow_frame.normal) - _, (center, radius, normal) = intersection_sphere_sphere(sphere1, sphere2) + sphere1_prim = (sphere1.base, sphere1.radius) + sphere2_prim = (sphere2.base, sphere2.radius) + + _, (center, radius, normal) = intersection_sphere_sphere(sphere1_prim, sphere2_prim) circle = ((center, normal), radius) intersect_pt1, intersect_pt2 = intersection_plane_circle(elbow_plane, circle) @@ -238,9 +241,4 @@ def inverse_kinematics_spherical_wrist(target_frame, points): axis6_angle = math.atan2(endy, endx) axis6_angles.append(axis6_angle) - return [ - [a1, a2, a3, a4, a5, a6] - for a1, a2, a3, a4, a5, a6 in zip( - axis1_angles, axis2_angles, axis3_angles, axis4_angles, axis5_angles, axis6_angles - ) - ] + return [[a1, a2, a3, a4, a5, a6] for a1, a2, a3, a4, a5, a6 in zip(axis1_angles, axis2_angles, axis3_angles, axis4_angles, axis5_angles, axis6_angles)] From c6a891284ce1f310ad539ffacc46487997ab8aed Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 16:18:52 +0100 Subject: [PATCH 15/36] updated changelog --- CHANGELOG.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2dee4023d9..7922926f79 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,24 @@ All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). +## [Unreleased] + +### Added + +* Added helper function `message` to `compas_fab.ghpython.components`. +* Added helper function `error` to `compas_fab.ghpython.components`. +* Added helper function `remark` to `compas_fab.ghpython.components`. +* Added helper function `warning` to `compas_fab.ghpython.components`. +* Added GH component definitions compatible with CPython in Rhino8. + +### Changed + +* Updated dev dependency to `compas_invocations2`. +* Fixed `AttributeError` in `inverse_kinematics_spherical_wrist()`. + +### Removed + + ## [1.0.2] 2024-02-22 ### Added From 148678ba2dc97273172c7753da6df7edca433bfe Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 16:39:36 +0100 Subject: [PATCH 16/36] warning using the new helper function --- .../ghpython/components_cpython/Cf_RosTopicSubscribe/code.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py index 7cca6727e7..bf416b6d25 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py @@ -15,14 +15,15 @@ from compas_fab.backends.ros.messages import ROSmsg from compas_fab.ghpython.components import create_id from compas_fab.ghpython.components import message +from compas_fab.ghpython.components import warning class ROSTopicSubscribe(Grasshopper.Kernel.GH_ScriptInstance): def RunScript(self, ros_client, topic_name: str, topic_type: str, interval: int, start: bool, stop: bool): if not topic_name: - raise ValueError("Please specify the name of the topic") + warning(ghenv.Component, "Please specify the name of the topic") # noqa: F821 if not topic_type: - raise ValueError("Please specify the type of the topic") + warning(ghenv.Component, "Please specify the type of the topic") # noqa: F821 if not hasattr(self, "message_count"): self.message_count = 0 From 1695cf03d6127f7055b23abfd56edb2782f5014e Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 16:39:56 +0100 Subject: [PATCH 17/36] added yak template and workflow --- .github/workflows/publish_yak.yml | 83 ++++++++++++++++++ src/compas_fab/ghpython/yak_template/icon.png | Bin 0 -> 4620 bytes .../ghpython/yak_template/manifest.yml | 22 +++++ tasks.py | 1 + 4 files changed, 106 insertions(+) create mode 100644 .github/workflows/publish_yak.yml create mode 100644 src/compas_fab/ghpython/yak_template/icon.png create mode 100644 src/compas_fab/ghpython/yak_template/manifest.yml diff --git a/.github/workflows/publish_yak.yml b/.github/workflows/publish_yak.yml new file mode 100644 index 0000000000..89bc070436 --- /dev/null +++ b/.github/workflows/publish_yak.yml @@ -0,0 +1,83 @@ +name: publish_yak + +on: + workflow_dispatch: + inputs: + environment: + description: "Choose deployment environment" + required: true + type: choice + options: + - test + - prod + +jobs: + + publish_test_yak: + runs-on: windows-latest + + steps: + + - name: Set test flag based on input + shell: pwsh + run: | + if ("${{ github.event.inputs.environment }}" -eq "test") { + echo "TEST_FLAG=--test-server" | Out-File -FilePath $env:GITHUB_ENV -Append + } + else { + echo "TEST_FLAG=" | Out-File -FilePath $env:GITHUB_ENV -Append + } + + - name: Checkout repo + uses: actions/checkout@v4 + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install -r requiremnets-dev.txt + + - name: Create CPython Grasshopper user objects + run: | + invoke build-cpython-ghuser-components + + - name: Create IronPython Grasshopper user objects + run: | + choco install ironpython --version=2.7.8.1 + invoke clean + invoke build-ghuser-components + + - name: Create Rhino7 Yak package + shell: pwsh + run: | + invoke yakerize -m $Env:YAK_TEMPLATE\manifest.yml -l $Env:YAK_TEMPLATE\icon.png -g $Env:USER_OBJECTS -t rh7 + env: + USER_OBJECTS: src\compas_fab\ghpython\components\ghuser + YAK_TEMPLATE: src\compas_fab\ghpython\yak_template + + - name: Publish to Yak server (Rhino 7) + shell: pwsh + run: | + $test_flag = if ($Env:TEST_FLAG) { $Env:TEST_FLAG } else { "" } + $file = Get-ChildItem -Path dist\yak_package\*rh7*.yak -File | Select-Object -ExpandProperty Name + $command = "invoke publish-yak -y dist\yak_package\$file $test_flag".Trim() + Invoke-Expression $command + env: + YAK_TOKEN: ${{ secrets.YAK_DF_TOKEN }} + + - name: Create Rhino8 Yak package + shell: pwsh + run: | + invoke yakerize -m $Env:YAK_TEMPLATE\manifest.yml -l $Env:YAK_TEMPLATE\icon.png -g $Env:USER_OBJECTS -t rh8 + env: + USER_OBJECTS: src\compas_fab\ghpython\components_cpython\ghuser + YAK_TEMPLATE: src\compas_fab\ghpython\yak_template + + - name: Publish to Yak server (Rhino 8) + shell: pwsh + run: | + $test_flag = if ($Env:TEST_FLAG) { $Env:TEST_FLAG } else { "" } + $file = Get-ChildItem -Path dist\yak_package\*rh8*.yak -File | Select-Object -ExpandProperty Name + $command = "invoke publish-yak -y dist\yak_package\$file $test_flag".Trim() + Invoke-Expression $command + env: + YAK_TOKEN: ${{ secrets.YAK_DF_TOKEN }} diff --git a/src/compas_fab/ghpython/yak_template/icon.png b/src/compas_fab/ghpython/yak_template/icon.png new file mode 100644 index 0000000000000000000000000000000000000000..e3ff78cde634215c7effbea1fa99b25431239893 GIT binary patch literal 4620 zcmV+n67%heP)fW=|IsbdkyPfw4DJ4G_L>|iIAw_8dIY2d;;-p8701zg{p?Ey?5>dDF#HdpFDC*BP zrk|34iOY&dC}#B+NI647wt$g9cn{55B^(lP03kMcqxI1f#pM@EX>Y`W@<_V zW8%XxDZN`19#wL%rj>2w4%QbpQqknsb8#Nj-0okh$TJ4R>n;;hG-={=d zxPNLQ(^4V~^_?!2jU`R2FK*=fsutQky<2HklVC!Uo!J@D%uJ8OW;X7!tz{0L-Co6s zpsgW3j4)ZR+jwJoYbgH{5&_wNKNxAZD(ev9SNivtXGHV+X^Ge^{=wC@xLLWso(~Hf zdJ z$@B@5OMUOmir}FO68X)@7_DBrNAl(mwY<5nuGe2XRD_uXH=Hw&MdJrx)oqV;g$?|5 zdo^xV_rHQ~>K&%b5C2<6TX%hSOMu8jnKYzm6@sgM?`&p;r++z^sk$Y9pt^-eH&;^D z*dFp^#wXgic5EC7Shc^Nebp@?w;dX7C2vMD*@OH><<>F>OTMX~)vaH3^1WTGc1`n=pgf)wJhU5iKNb=6Oo1azpn4S)%}%l_F}Lo{KhN_b{l z6(1Hhggj{Q+6U{d9^Pr$zr25xisnxBeV3R&I+pvVCgL+Qc2+pK=ZkV&{tu!`sb040 zmeCtJYae9508CtVC|;Ff6&;fYc<6#e!{i^?RMG3n1N?SWOqXT99@%wSS6EZfz$2Sa z;+yZ}!FKMOnxLd1FBu+X5O~k;sOHO~O}!p0KvR3y znxVOmTI0(jjXblhN_(E070c|5XnmC==~kZVtZfhhlU5v>ECe_Co>QVNJbYo2R=#q7 z9q<2G-=~2BtUg#zXG+2&C2J1$z26TD8+cDQt_Php!2r`X@2P3#th2d5?KixpKU&jhwH z5Rmg)(M%-Me9tq|BDr9wci($dN#5p4y>RIhuE>lbEyju};o1CZp3ARB0>fgh%o-8h zWBcylRwYkwt-{kG*r%pMFg-O=UuF7~R}W3szc3J>O0mTETL8D6AFq{}6v5NKOlHau zqsiMF*ewdTcl7z`q0@ZuV?FQxSkD(7mfT&F1_hbCp+W$&Gotz1r74(%-$=T1Vmx|B zR4Vch`gYpCO?{&<9i(RJUX&K8IW`Tg9yXRVF+47e7cWcUrP)In7HjSGAQzuIka)Wp zr%UCPJ^roq;?7gFxK$D&%q$!~px6Co#fP(OPAYj9Co|Ay=8GduG_<+_7#<8*6fr<`uy+E)j_qVWSrj&Dx56*sD#HhpQNc><(jc^ zth;(R3&zILQ@rEuDG69jg7QWe?{^~Y-`-cp$z~T}Ca;Cp9W07q!Pq!f&mYE>S+UeO z-8{DWB)9*gj6=0v(|>J211%jE;e;f+kyL>;R|O#Fje<;Igzx3-jA(5sY%FOqcy+3T z&x#wFzvdWk?W@CXQMflJfj54erjZtiiG%H2oaSAI&+n-2RNA+DBrojr26ZkO9>s(% z`kI~^$p`a?ac@onCIQR$)-rGPQ9dhfGz3Zk`Wh% zN0lti@7dW|nqQ47Vbs7d=45)U&wFwbcyI17rlv%2@Kh_ee0rQE-&An22ftzWNvF2j zvIm75;ucb6rbd8}a-6SVT!IaA2lYSnB9gPYw3%zxmasIx8n+5jVG2=UCVn@;tM=9u zG;pN8Th|ty1hBO_kOHr!_g*M-72?Fj3+kSL}QzWSNGP2+|PgS zuHje1qlvSbxar)1tk_%2itlUt6k#f;Y32N6I{;}hR(70l03alpA`eB$Nh?f6cU+aG zBcSwj=m8ErYFpe4iZC-bD^{COyxmOR#mOW@n7eE@G}_9uvxblqX<@})ubr@9Y@B90 z^ac7kq{ZlL7lCa;O2JUY<5CPdCo#e=z&f+pL&d#Q5-=-*V-4-BD>}`>adFJfh-UTt zVca}npdsQLVNtki@*v)um(E40kz6$b?puprACs|A$6DLSL0D7Pj0Ew1l(=s#k2OR;mpa5=JJea)?PJ? zh2sX$>QY&nU(J(Ss&K24zjZ{2rl&?4LU6r7Q@cm|Z4WToJP>78Axdw<*mMN6^kLaq z6v15`Uu;upv$p4a^5LsvP24##p35_1c;q7Qxr#@Xd~mRyWqWFAZ1WFzYng*>9H}u@T&m>7ox!J7HBL8q-&V5tlTuu& z#Nkr8?xRwk-Ck{&ybsT9ug0ZH(qpa68y(x-vc`5+AM~olTPF-8Hr&)@-Oix@>Oq23m5qv&!a#sSS1Bq) zN5==B-mYtL^X3nAy=vPXmhY*dw#7|Un2B4@i|@IC4pCM=VU7TqW`ZWgWKj+4v{GkX zr9_AB^6D+Omv+?{h&)p`Tq?`=cFJy*#msGdtq%WpB0}Dxm~FEsHlO$npUrGF(skr1|87P!C==&TK1NYE>k*AO|%@G zV-5aFW8%9{=UFs<0G})v&c;QVOh~dJfu-B4Im4(~r%nhQS)V(CJ0^C& zB}VJ!P}1O+HBxX`K8otI4E6!p0Q;+3aH|rtA{d`&!)g+p`UC$lJ;@L%2XJ*9gNKSc z)|a97VyEn@YQgQ1SWE(&S;1yjxbd8Uq{diz=6^y^!n&@Dd>9 zW`xKEucR!fX=Pl3O*^9Y>2e<|Cc&>WgAXyRCc&~fsfIra1p&T#m&i`AVd*4iU}kz0 z5f&v#?o3R!Ylo=&PqiAffKb>79RzrU`i;qd2*lPhhsNCOjOZW&#tsTcbP)j>pJ-!z zV&}n(p9%sPmuMru+&E4>vD?dxWl|`90(Rd#rg+MVL&XxA0p38uZRf`$fSKu$JhM$P zuy>@cYyDo*&>oW8+ZPOv3+pl%TU2ZO&j^d6MWIwlU=G0{U%H*&JCN#1gm@I7qRGYH zN+;(f*|C|u$q8@o^Xv7db`RfGIGH%uZunEDOXd2HO1g~D1j1*egzD*qn-xCK9T8;G z?y7K7-(pm8^T)($A@;qMPGj&y@{aGP){|o94e*E6RvpkqRkP!Q@^$W5U&cQUH{x)q zw0R^2HLYB;_E^Z12l(vT5v-m+EadvFZk4Oo9;2Y9l{SypjQ*d)r@3=|nW3J|tdKj_ zAJu;MfFTGgF)1s34~FCM$tw11@A4J_YV%kn*Eb}SZ%O^Y&dxKyrK zb);YWy?tW5W|AMSYh&TM679=M_;T0HqqJ-n!&*{={4@TFyt3Or;Ty&eU}!Ha!?T67 z7%SK5&NN=$U1NwA3zR4I^$i5%FCMiK!8YIX=F(=ibVPBjCczUklCktM;qm8$SrI&P zQIe+m^y3E*ux01XncwQ$1bI7BihIDHQ}bMYwI-y-3<~e@(Bf?3?kR&b_3h{guIQj5 zu%I;5GIwG0}ruuUM))X`Z>itS#VM$O! zx**`YTSwL?LS74KA)3EztJ2a_6#-AoNH%0>oh@87CYHR5leJT%tz{0L3t0M6s*83k z%IY*O-(|!jfAOe|fTk5x34h#lQadRUcvr%`ISFS)Fj`H5`==)GfG$Jq`^pxUd{co( z=Z*>~pXe$>47wozU|0F5N0EHsdv0~B-1T`mTXlBzg0XQdo0CdvFJcF0inJIj%jcvT z_WYLP&D`}xc|f8h@9#X3^>}xEbxVMJ6xA8yxn9DTzIUx|l?T2$VMvi1J1CrYuT1B* z^Wq8bV}?g4u$dKZpBT@(bJGoe{+faY9{AdrB`RTKz0-6R40IbFy@(mSBWMVg8s{aQde+JqEMA$>qo+$#$&nT= z%ZO%LN(5sEg=;wnp~Bq}!`oiw;PWGmT_r=Dol8@C^yb=JT8#JFTvCJ?n?=zMVjV7( z7MDt;!%ay;J0%Tm90-Wv1q~dw2&q1B)~?M3V$!RHGlWnc?#MO#|8sFN5I@D$S>Od%{0x_;O(u1wM9vKP^0cG{!676Egu+HCl#Ts;Us#Wylz^Z=%R|XY zD@>Jc2C71dl~P6m2qBsksWu2RwfUu4$A8Z2^Zx{R6Nb5+bJlzS0000 Date: Mon, 24 Mar 2025 16:56:50 +0100 Subject: [PATCH 18/36] run workflows on LTS branch as well --- .github/workflows/build.yml | 2 ++ .github/workflows/docs.yml | 1 + .github/workflows/integration.yml | 2 ++ .github/workflows/ironpython.yml | 2 ++ .github/workflows/pr-checks.yml | 1 + 5 files changed, 8 insertions(+) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index dbabe5c943..a34f7ddb9a 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -4,9 +4,11 @@ on: push: branches: - main + - LTS-main-1.x pull_request: branches: - main + - LTS-main-1.x jobs: build: diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index 803a09b602..5a098c527c 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -4,6 +4,7 @@ on: push: branches: - main + - LTS-main-1.x tags: - 'v*' pull_request_review: diff --git a/.github/workflows/integration.yml b/.github/workflows/integration.yml index a915724c56..c3642d853a 100644 --- a/.github/workflows/integration.yml +++ b/.github/workflows/integration.yml @@ -4,9 +4,11 @@ on: push: branches: - main + - LTS-main-1.x pull_request: branches: - main + - LTS-main-1.x jobs: build: diff --git a/.github/workflows/ironpython.yml b/.github/workflows/ironpython.yml index db6c9602c8..0bd4d17003 100644 --- a/.github/workflows/ironpython.yml +++ b/.github/workflows/ironpython.yml @@ -4,9 +4,11 @@ on: push: branches: - main + - LTS-main-1.x pull_request: branches: - main + - LTS-main-1.x jobs: build: diff --git a/.github/workflows/pr-checks.yml b/.github/workflows/pr-checks.yml index a61aab5bdc..23a58cecd1 100644 --- a/.github/workflows/pr-checks.yml +++ b/.github/workflows/pr-checks.yml @@ -4,6 +4,7 @@ on: types: [assigned, opened, synchronize, reopened, labeled, unlabeled] branches: - main + - LTS-main-1.x jobs: build: From b5e0e2471b49fbf250157c20df64142241967851 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 16:58:54 +0100 Subject: [PATCH 19/36] fixed typo in requirements --- requirements-dev.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements-dev.txt b/requirements-dev.txt index ca0d48fc35..b7cacc27b2 100644 --- a/requirements-dev.txt +++ b/requirements-dev.txt @@ -3,7 +3,7 @@ autopep8 black bump2version >=1.0.1 check-manifest >=0.36 -compas_invocations2 +compas-invocations2 doc8 flake8 importlib_metadata <5.0 From 51acb999f3e925d83f621837de9969ced7a35e6b Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 17:11:30 +0100 Subject: [PATCH 20/36] removed python 3.8 from workflow, added 3.11, 3.12, 3.13 --- .github/workflows/build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index a34f7ddb9a..8c5e3cb7ed 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -17,7 +17,7 @@ jobs: strategy: matrix: os: [ubuntu-latest, macos-latest, windows-latest] - python: ['3.8', '3.9', '3.10'] + python: ['3.9', '3.10', '3.11', '3.12', '3.13'] steps: - uses: compas-dev/compas-actions.build@v3 From 4e42274a5c0215bdc792701648b6bcc5456a39fa Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 17:30:02 +0100 Subject: [PATCH 21/36] add ruff and make it happy --- pyproject.toml | 31 +++++++++++++++++++ requirements-dev.txt | 1 + src/compas_fab/backends/interfaces/client.py | 1 + .../pybullet_inverse_kinematics.py | 6 ++-- .../components_cpython/Cf_AttachTool/code.py | 1 - .../Cf_AttachedCollisionMesh/code.py | 4 +-- .../Cf_CollisionMesh/code.py | 5 ++- .../Cf_ConstraintsFromConfiguration/code.py | 3 +- .../Cf_ConstraintsFromPlane/code.py | 4 +-- .../Cf_InverseKinematics/code.py | 1 - .../Cf_PlanCartesianMotion/code.py | 4 +-- .../components_cpython/Cf_PlanMotion/code.py | 3 +- .../Cf_PlanningScene/code.py | 1 - .../components_cpython/Cf_RosConnect/code.py | 1 - .../components_cpython/Cf_RosRobot/code.py | 1 - .../Cf_RosTopicPublish/code.py | 3 +- .../Cf_RosTopicSubscribe/code.py | 1 - .../Cf_VisualizeRobot/code.py | 1 - .../Cf_VisualizeTrajectory/code.py | 1 - 19 files changed, 43 insertions(+), 30 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index db1ccb638e..8e4889f93f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -24,3 +24,34 @@ known_first_party = "compas_fab" default_section = "THIRDPARTY" forced_separate = "test_compas_fab" skip = ["__init__.py"] + +[tool.ruff] +line-length = 120 +indent-width = 4 +target-version = "py39" + +[tool.ruff.lint] +select = ["E", "F", "I"] + +[tool.ruff.lint.per-file-ignores] +"__init__.py" = ["I001"] +"tests/*" = ["I001"] +"tasks.py" = ["I001"] +"src/compas_fab/ghpython/components/*/code.py" = ["F821", "F401"] +"src/compas_fab/ghpython/components_cpython/*/code.py" = ["F821", "F401"] + +[tool.ruff.lint.isort] +force-single-line = true +known-first-party = [ + "compas_fab" +] + +[tool.ruff.lint.pydocstyle] +convention = "numpy" + +[tool.ruff.lint.pycodestyle] +max-line-length = 179 + +[tool.ruff.format] +docstring-code-format = true +docstring-code-line-length = "dynamic" diff --git a/requirements-dev.txt b/requirements-dev.txt index b7cacc27b2..f1bd04139d 100644 --- a/requirements-dev.txt +++ b/requirements-dev.txt @@ -6,6 +6,7 @@ check-manifest >=0.36 compas-invocations2 doc8 flake8 +ruff importlib_metadata <5.0 invoke>=0.14 isort diff --git a/src/compas_fab/backends/interfaces/client.py b/src/compas_fab/backends/interfaces/client.py index 057c04e53f..e678da8c34 100644 --- a/src/compas_fab/backends/interfaces/client.py +++ b/src/compas_fab/backends/interfaces/client.py @@ -1,6 +1,7 @@ from __future__ import absolute_import from __future__ import division from __future__ import print_function + from compas_fab.backends.exceptions import BackendFeatureNotSupportedError diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py index 0a6eaa1153..a57cd5354b 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py @@ -106,7 +106,7 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N # I don't know what jointRanges needs to be. Erwin Coumans knows, but he isn't telling. # https://stackoverflow.com/questions/49674179/understanding-inverse-kinematics-pybullet # https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/preview?pru=AAABc7276PI*zazLer2rlZ8tAUI8lF98Kw#heading=h.9i02ojf4k3ve - joint_ranges = [u - l for u, l in zip(upper_limits, lower_limits)] + joint_ranges = [upper - lower for upper, lower in zip(upper_limits, lower_limits)] if options.get("semi-constrained"): ik_options.update( @@ -213,7 +213,5 @@ def _accurate_inverse_kinematics(self, joints, threshold, max_iter, **kwargs): return joint_poses, close_enough def _get_rest_poses(self, joint_names, configuration): - name_value_map = { - configuration.joint_names[i]: configuration.joint_values[i] for i in range(len(configuration.joint_names)) - } + name_value_map = {configuration.joint_names[i]: configuration.joint_values[i] for i in range(len(configuration.joint_names))} return [name_value_map[name] for name in joint_names] diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py index 103ab074ee..0f953da8b8 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py @@ -7,7 +7,6 @@ import Grasshopper import Rhino - from compas.geometry import Frame from compas_rhino.conversions import mesh_to_compas from compas_rhino.conversions import plane_to_compas_frame diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py index 9cb0401fee..8e471d1f0c 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py @@ -5,11 +5,9 @@ COMPAS FAB v1.0.2 """ -import System - import Grasshopper import Rhino - +import System from compas_rhino.conversions import mesh_to_compas from compas_fab.robots import AttachedCollisionMesh diff --git a/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py index 1f1031922e..fe6d73e2c7 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_CollisionMesh/code.py @@ -7,12 +7,11 @@ import Grasshopper import Rhino - from compas_rhino.conversions import mesh_to_compas -from compas_fab.robots import CollisionMesh -from compas_fab.ghpython.components import message from compas_fab.ghpython.components import error +from compas_fab.ghpython.components import message +from compas_fab.robots import CollisionMesh class CollisionMeshComponent(Grasshopper.Kernel.GH_ScriptInstance): diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py index 91591318c0..4160b59293 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromConfiguration/code.py @@ -7,9 +7,8 @@ import math -import System - import Grasshopper +import System class ConstraintsFromTargetConfiguration(Grasshopper.Kernel.GH_ScriptInstance): diff --git a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/code.py b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/code.py index cf978fca81..9d14324377 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_ConstraintsFromPlane/code.py @@ -7,10 +7,8 @@ import math -import System - import Grasshopper - +import System from compas_rhino.conversions import plane_to_compas_frame diff --git a/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/code.py b/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/code.py index d21948d2ac..8a269d5401 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_InverseKinematics/code.py @@ -6,7 +6,6 @@ """ import Grasshopper - from compas_rhino.conversions import plane_to_compas_frame diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py index 72bd11dc67..61255c037c 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py @@ -5,10 +5,8 @@ COMPAS FAB v1.0.2 """ -import System - import Grasshopper - +import System from compas_rhino.conversions import plane_to_compas_frame from scriptcontext import sticky as st diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py index 42fd7c5791..f7d28be676 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py @@ -5,9 +5,8 @@ COMPAS FAB v1.0.2 """ -import System import Grasshopper - +import System from scriptcontext import sticky as st from compas_fab.ghpython.components import create_id diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py index d1131ce319..227d7e690f 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py @@ -6,7 +6,6 @@ """ import Grasshopper - from scriptcontext import sticky as st from compas_fab.ghpython.components import create_id diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py index 6ded731250..e970bb63ba 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py @@ -6,7 +6,6 @@ """ import Grasshopper - from scriptcontext import sticky as st from compas_fab.backends import RosClient diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py index d2df6928d4..1c0cd8ce80 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py @@ -6,7 +6,6 @@ """ import Grasshopper - from compas.scene import SceneObject from scriptcontext import sticky as st diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py index ea3b402e26..78880ff8b2 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py @@ -8,14 +8,13 @@ import time import Grasshopper - from roslibpy import Topic from scriptcontext import sticky as st from compas_fab.backends.ros.messages import ROSmsg from compas_fab.ghpython.components import create_id -from compas_fab.ghpython.components import warning from compas_fab.ghpython.components import message +from compas_fab.ghpython.components import warning class ROSTopicPublish(Grasshopper.Kernel.GH_ScriptInstance): diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py index bf416b6d25..9e028f233d 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py @@ -8,7 +8,6 @@ import time import Grasshopper - from roslibpy import Topic from scriptcontext import sticky as st diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py index 9d6a290ed7..dbbd20ebe4 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py @@ -8,7 +8,6 @@ import time import Grasshopper - from compas.geometry import Frame from compas.geometry import Transformation from compas.scene import SceneObject diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py index cd923ec502..16ed5cd645 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py @@ -6,7 +6,6 @@ """ import Grasshopper - from compas_ghpython.drawing import draw_frame from compas_ghpython.sets import list_to_ghtree From f5de604c00193a42d5a8c5d2077b084a085bdc02 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 17:32:37 +0100 Subject: [PATCH 22/36] make formatter happy --- .../kinematics/solvers/spherical_wrist.py | 7 ++- .../pybullet_inverse_kinematics.py | 4 +- src/compas_fab/backends/pybullet/client.py | 4 +- src/compas_fab/backends/pybullet/utils.py | 8 ++-- src/compas_fab/backends/ros/client.py | 4 +- .../backends/ros/messages/geometry_msgs.py | 2 +- .../components_cpython/Cf_AttachTool/code.py | 10 +++- .../Cf_AttachedCollisionMesh/code.py | 11 ++++- .../components_cpython/Cf_PlanMotion/code.py | 9 +++- .../Cf_RosTopicSubscribe/code.py | 4 +- .../Cf_VisualizeRobot/code.py | 4 +- .../Cf_VisualizeTrajectory/code.py | 4 +- src/compas_fab/robots/constraints.py | 18 +++---- src/compas_fab/robots/inertia.py | 2 +- src/compas_fab/robots/planning_scene.py | 46 +++++++++--------- src/compas_fab/robots/robot.py | 48 ++++++++++--------- src/compas_fab/robots/tool.py | 10 ++-- src/compas_fab/robots/wrench.py | 6 +-- src/compas_fab/sensors/base.py | 6 +-- src/compas_fab/sensors/baumer.py | 4 +- 20 files changed, 125 insertions(+), 86 deletions(-) diff --git a/src/compas_fab/backends/kinematics/solvers/spherical_wrist.py b/src/compas_fab/backends/kinematics/solvers/spherical_wrist.py index a555d24116..a2c45b5f73 100644 --- a/src/compas_fab/backends/kinematics/solvers/spherical_wrist.py +++ b/src/compas_fab/backends/kinematics/solvers/spherical_wrist.py @@ -241,4 +241,9 @@ def inverse_kinematics_spherical_wrist(target_frame, points): axis6_angle = math.atan2(endy, endx) axis6_angles.append(axis6_angle) - return [[a1, a2, a3, a4, a5, a6] for a1, a2, a3, a4, a5, a6 in zip(axis1_angles, axis2_angles, axis3_angles, axis4_angles, axis5_angles, axis6_angles)] + return [ + [a1, a2, a3, a4, a5, a6] + for a1, a2, a3, a4, a5, a6 in zip( + axis1_angles, axis2_angles, axis3_angles, axis4_angles, axis5_angles, axis6_angles + ) + ] diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py index a57cd5354b..eb874fedb1 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py @@ -213,5 +213,7 @@ def _accurate_inverse_kinematics(self, joints, threshold, max_iter, **kwargs): return joint_poses, close_enough def _get_rest_poses(self, joint_names, configuration): - name_value_map = {configuration.joint_names[i]: configuration.joint_values[i] for i in range(len(configuration.joint_names))} + name_value_map = { + configuration.joint_names[i]: configuration.joint_values[i] for i in range(len(configuration.joint_names)) + } return [name_value_map[name] for name in joint_names] diff --git a/src/compas_fab/backends/pybullet/client.py b/src/compas_fab/backends/pybullet/client.py index e3cedc7d1f..72eb2bc154 100644 --- a/src/compas_fab/backends/pybullet/client.py +++ b/src/compas_fab/backends/pybullet/client.py @@ -139,8 +139,8 @@ class PyBulletClient(PyBulletBase, ClientInterface): Examples -------- >>> from compas_fab.backends import PyBulletClient - >>> with PyBulletClient(connection_type='direct') as client: - ... print('Connected: %s' % client.is_connected) + >>> with PyBulletClient(connection_type="direct") as client: + ... print("Connected: %s" % client.is_connected) Connected: True """ diff --git a/src/compas_fab/backends/pybullet/utils.py b/src/compas_fab/backends/pybullet/utils.py index f818ecc548..b87b3e78a0 100644 --- a/src/compas_fab/backends/pybullet/utils.py +++ b/src/compas_fab/backends/pybullet/utils.py @@ -25,10 +25,10 @@ def redirect_stdout(to=os.devnull, enabled=True): Examples -------- - >>> import os # doctest: +SKIP - >>> with redirect_stdout(to='filename'): # doctest: +SKIP - ... print("from Python") # doctest: +SKIP - ... os.system("echo non-Python applications are also supported") # doctest: +SKIP + >>> import os # doctest: +SKIP + >>> with redirect_stdout(to="filename"): # doctest: +SKIP + ... print("from Python") # doctest: +SKIP + ... os.system("echo non-Python applications are also supported") # doctest: +SKIP """ def _redirect_stdout(to_): diff --git a/src/compas_fab/backends/ros/client.py b/src/compas_fab/backends/ros/client.py index ce9edbd626..e725ba2142 100644 --- a/src/compas_fab/backends/ros/client.py +++ b/src/compas_fab/backends/ros/client.py @@ -82,7 +82,7 @@ def from_local_cache_directory(cls, local_cache_directory): False - >>> local_directory = os.path.join(os.path.expanduser('~'), 'robot_description', 'robocop') + >>> local_directory = os.path.join(os.path.expanduser("~"), "robot_description", "robocop") >>> info = LocalCacheInfo.from_local_cache_directory(local_directory) >>> info.use_local_cache True @@ -126,7 +126,7 @@ class RosClient(Ros, ClientInterface): >>> from compas_fab.backends import RosClient >>> with RosClient() as client: - ... print('Connected: %s' % client.is_connected) + ... print("Connected: %s" % client.is_connected) Connected: True Notes diff --git a/src/compas_fab/backends/ros/messages/geometry_msgs.py b/src/compas_fab/backends/ros/messages/geometry_msgs.py index e12a7bfa03..c5a9173cef 100644 --- a/src/compas_fab/backends/ros/messages/geometry_msgs.py +++ b/src/compas_fab/backends/ros/messages/geometry_msgs.py @@ -201,7 +201,7 @@ class Inertia(ROSmsg): Examples -------- - >>> inertia = compas_fab.robots.Inertia([[0] * 3] * 3, 1., [0.1, 3.1, 4.4]) + >>> inertia = compas_fab.robots.Inertia([[0] * 3] * 3, 1.0, [0.1, 3.1, 4.4]) >>> ros_inertia = Inertia.from_inertia(inertia) >>> ros_inertia.msg {'m': 1.0, 'com': {'x': 0.1, 'y': 3.1, 'z': 4.4}, 'ixx': 0.0, 'ixy': 0.0, 'ixz': 0.0, 'iyy': 0.0, 'iyz': 0.0, 'izz': 0.0} diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py index 0f953da8b8..139bf2d270 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachTool/code.py @@ -15,7 +15,15 @@ class AttachToolComponent(Grasshopper.Kernel.GH_ScriptInstance): - def RunScript(self, robot, visual_mesh: Rhino.Geometry.Mesh, collision_mesh: Rhino.Geometry.Mesh, tcf_plane, group, connected_to): + def RunScript( + self, + robot, + visual_mesh: Rhino.Geometry.Mesh, + collision_mesh: Rhino.Geometry.Mesh, + tcf_plane, + group, + connected_to, + ): if robot and robot.client and robot.client.is_connected and visual_mesh: if not collision_mesh: collision_mesh = visual_mesh diff --git a/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py index 8e471d1f0c..e196a6692b 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_AttachedCollisionMesh/code.py @@ -15,7 +15,16 @@ class AttachedCollisionMeshComponent(Grasshopper.Kernel.GH_ScriptInstance): - def RunScript(self, scene, mesh: Rhino.Geometry.Mesh, identifier: str, link_name: str, touch_links: System.Collections.Generic.List[str], add: bool, remove: bool): + def RunScript( + self, + scene, + mesh: Rhino.Geometry.Mesh, + identifier: str, + link_name: str, + touch_links: System.Collections.Generic.List[str], + add: bool, + remove: bool, + ): attached_collision_mesh = None if scene and mesh and identifier and link_name: compas_mesh = mesh_to_compas(mesh) diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py index f7d28be676..1ef5a51a68 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py @@ -30,7 +30,14 @@ def RunScript( attached_collision_meshes = list(attached_collision_meshes) if attached_collision_meshes else None planner_id = str(planner_id) if planner_id else "RRTConnect" - if robot and robot.client and robot.client.is_connected and start_configuration and goal_constraints and compute: + if ( + robot + and robot.client + and robot.client.is_connected + and start_configuration + and goal_constraints + and compute + ): st[key] = robot.plan_motion( goal_constraints, start_configuration=start_configuration, diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py index 9e028f233d..e491df60cb 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py @@ -75,7 +75,9 @@ def topic_callback(self, msg): ghdoc = ghenv.Component.OnPingDocument() # noqa: F821 This is defined by Grasshopper if not self.is_updating and ghdoc.SolutionState != Grasshopper.Kernel.GH_ProcessStep.Process: self.is_updating = True - ghdoc.ScheduleSolution(self.interval, Grasshopper.Kernel.GH_Document.GH_ScheduleDelegate(self.expire_callback)) + ghdoc.ScheduleSolution( + self.interval, Grasshopper.Kernel.GH_Document.GH_ScheduleDelegate(self.expire_callback) + ) def expire_callback(self, ghdoc): if ghdoc.SolutionState != Grasshopper.Kernel.GH_ProcessStep.Process: diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py index dbbd20ebe4..718727c977 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py @@ -94,7 +94,9 @@ def RunScript( try: scene = robot.client.get_planning_scene() except BackendFeatureNotSupportedError: - print("The selected backend does not support collision meshes. If you need collision mesh support, use a different backend.") + print( + "The selected backend does not support collision meshes. If you need collision mesh support, use a different backend." + ) scene = None show_cm = False show_acm = False diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py index 16ed5cd645..306a13135a 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeTrajectory/code.py @@ -26,7 +26,9 @@ def RunScript(self, robot, group, trajectory): group = group or robot.main_group_name for c in trajectory.points: - configurations.append(robot.merge_group_with_full_configuration(c, trajectory.start_configuration, group)) + configurations.append( + robot.merge_group_with_full_configuration(c, trajectory.start_configuration, group) + ) frame = robot.forward_kinematics(c, group, options=dict(solver="model")) planes.append(draw_frame(frame)) positions.append(c.positions) diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index 88465e8a4e..f19a42fff9 100644 --- a/src/compas_fab/robots/constraints.py +++ b/src/compas_fab/robots/constraints.py @@ -75,7 +75,7 @@ def from_box(cls, box): >>> from compas.geometry import Frame >>> from compas.geometry import Box >>> from compas_fab.robots import BoundingVolume - >>> box = Box(1., 1., 1.) + >>> box = Box(1.0, 1.0, 1.0) >>> bv = BoundingVolume.from_box(box) >>> bv.type 1 @@ -124,7 +124,7 @@ def from_mesh(cls, mesh): >>> import compas >>> from compas.datastructures import Mesh >>> from compas_fab.robots import BoundingVolume - >>> mesh = Mesh.from_obj(compas.get('faces.obj')) + >>> mesh = Mesh.from_obj(compas.get("faces.obj")) >>> bv = BoundingVolume.from_mesh(Mesh) >>> bv.type 3 @@ -437,8 +437,8 @@ class PositionConstraint(Constraint): >>> from compas.geometry import Sphere >>> from compas_fab.robots import PositionConstraint >>> from compas_fab.robots import BoundingVolume - >>> bv = BoundingVolume.from_sphere(Sphere(0.5, point=[3,4,5])) - >>> pc = PositionConstraint('link_0', bv, weight=1.) + >>> bv = BoundingVolume.from_sphere(Sphere(0.5, point=[3, 4, 5])) + >>> pc = PositionConstraint("link_0", bv, weight=1.0) """ def __init__(self, link_name, bounding_volume, weight=1.0): @@ -470,7 +470,7 @@ def from_box(cls, link_name, box, weight=1.0): >>> from compas.geometry import Frame >>> from compas.geometry import Box >>> box = Box(4, 4, 4, Frame.worldXY()) - >>> pc = PositionConstraint.from_box('link_0', box) + >>> pc = PositionConstraint.from_box("link_0", box) """ bounding_volume = BoundingVolume.from_box(box) return cls(link_name, bounding_volume, weight) @@ -497,8 +497,8 @@ def from_sphere(cls, link_name, sphere, weight=1.0): -------- >>> from compas_fab.robots import PositionConstraint >>> from compas.geometry import Sphere - >>> sphere = Sphere(radius=0.5, point=[3,4,5]) - >>> pc = PositionConstraint.from_sphere('link_0', sphere, weight=1.) + >>> sphere = Sphere(radius=0.5, point=[3, 4, 5]) + >>> pc = PositionConstraint.from_sphere("link_0", sphere, weight=1.0) """ bounding_volume = BoundingVolume.from_sphere(sphere) return cls(link_name, bounding_volume, weight) @@ -526,8 +526,8 @@ def from_mesh(cls, link_name, mesh, weight=1.0): -------- >>> from compas.datastructures import Mesh >>> import compas - >>> mesh = Mesh.from_obj(compas.get('faces.obj')) - >>> pc = PositionConstraint.from_mesh('link_0', mesh) + >>> mesh = Mesh.from_obj(compas.get("faces.obj")) + >>> pc = PositionConstraint.from_mesh("link_0", mesh) """ bounding_volume = BoundingVolume.from_mesh(mesh) return cls(link_name, bounding_volume, weight) diff --git a/src/compas_fab/robots/inertia.py b/src/compas_fab/robots/inertia.py index 960a1de372..14e442bf6f 100644 --- a/src/compas_fab/robots/inertia.py +++ b/src/compas_fab/robots/inertia.py @@ -30,7 +30,7 @@ class Inertia: Examples -------- - >>> inertia = Inertia([[0] * 3] * 3, 1., Point(0.1, 3.1, 4.4)) + >>> inertia = Inertia([[0] * 3] * 3, 1.0, Point(0.1, 3.1, 4.4)) >>> inertia Inertia([[0, 0, 0], [0, 0, 0], [0, 0, 0]], 1.0, Point(x=0.1, y=3.1, z=4.4)) >>> inertia.principal_moments diff --git a/src/compas_fab/robots/planning_scene.py b/src/compas_fab/robots/planning_scene.py index 7586e558f9..f846762c5c 100644 --- a/src/compas_fab/robots/planning_scene.py +++ b/src/compas_fab/robots/planning_scene.py @@ -49,8 +49,8 @@ class CollisionMesh(Data): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/floor.stl')) - >>> cm = CollisionMesh(mesh, 'floor') + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) + >>> cm = CollisionMesh(mesh, "floor") >>> cm.frame Frame(point=Point(x=0.0, y=0.0, z=0.0), xaxis=Vector(x=1.0, y=0.0, z=0.0), yaxis=Vector(x=0.0, y=1.0, z=0.0)) """ @@ -149,10 +149,10 @@ class AttachedCollisionMesh(Data): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) - >>> cm = CollisionMesh(mesh, 'tip') - >>> ee_link_name = 'ee_link' - >>> touch_links = ['wrist_3_link', 'ee_link'] + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) + >>> cm = CollisionMesh(mesh, "tip") + >>> ee_link_name = "ee_link" + >>> touch_links = ["wrist_3_link", "ee_link"] >>> acm = AttachedCollisionMesh(cm, ee_link_name, touch_links) """ @@ -260,9 +260,9 @@ def add_collision_mesh(self, collision_mesh, scale=False): Examples -------- >>> scene = PlanningScene(robot) - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/floor.stl')) - >>> cm = CollisionMesh(mesh, 'floor') - >>> scene.add_collision_mesh(cm) # doctest: +SKIP + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) + >>> cm = CollisionMesh(mesh, "floor") + >>> scene.add_collision_mesh(cm) # doctest: +SKIP """ self.ensure_client() @@ -289,7 +289,7 @@ def remove_collision_mesh(self, id): Examples -------- >>> scene = PlanningScene(robot) - >>> scene.remove_collision_mesh('floor') # doctest: +SKIP + >>> scene.remove_collision_mesh("floor") # doctest: +SKIP """ self.ensure_client() self.robot.client.remove_collision_mesh(id) @@ -321,9 +321,9 @@ def append_collision_mesh(self, collision_mesh, scale=False): Examples -------- >>> scene = PlanningScene(robot) - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/floor.stl')) - >>> cm = CollisionMesh(mesh, 'floor') - >>> scene.append_collision_mesh(cm) # doctest: +SKIP + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) + >>> cm = CollisionMesh(mesh, "floor") + >>> scene.append_collision_mesh(cm) # doctest: +SKIP """ self.ensure_client() @@ -355,12 +355,12 @@ def add_attached_collision_mesh(self, attached_collision_mesh, scale=False): Examples -------- >>> scene = PlanningScene(robot) - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) - >>> cm = CollisionMesh(mesh, 'tip') - >>> ee_link_name = 'ee_link' - >>> touch_links = ['wrist_3_link', 'ee_link'] + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) + >>> cm = CollisionMesh(mesh, "tip") + >>> ee_link_name = "ee_link" + >>> touch_links = ["wrist_3_link", "ee_link"] >>> acm = AttachedCollisionMesh(cm, ee_link_name, touch_links) - >>> scene.add_attached_collision_mesh(acm) # doctest: +SKIP + >>> scene.add_attached_collision_mesh(acm) # doctest: +SKIP """ self.ensure_client() @@ -387,7 +387,7 @@ def remove_attached_collision_mesh(self, id): Examples -------- >>> scene = PlanningScene(robot) - >>> scene.remove_attached_collision_mesh('tip') # doctest: +SKIP + >>> scene.remove_attached_collision_mesh("tip") # doctest: +SKIP """ self.ensure_client() self.client.remove_attached_collision_mesh(id) @@ -413,11 +413,11 @@ def attach_collision_mesh_to_robot_end_effector(self, collision_mesh, scale=Fals Examples -------- >>> scene = PlanningScene(robot) - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) - >>> cm = CollisionMesh(mesh, 'tip') + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) + >>> cm = CollisionMesh(mesh, "tip") >>> group = robot.main_group_name - >>> scene.attach_collision_mesh_to_robot_end_effector(cm, group=group) # attach to ee - >>> scene.remove_attached_collision_mesh('tip') # now detach + >>> scene.attach_collision_mesh_to_robot_end_effector(cm, group=group) # attach to ee + >>> scene.remove_attached_collision_mesh("tip") # now detach """ self.ensure_client() diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 68152d951b..940ab309fa 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -131,7 +131,7 @@ def basic(cls, name, joints=None, links=None, materials=None, **kwargs): Examples -------- - >>> robot = Robot.basic('A robot') + >>> robot = Robot.basic("A robot") >>> robot.name 'A robot' """ @@ -179,7 +179,7 @@ def group_states(self): Examples -------- - >>> sorted(robot.group_states['manipulator'].keys()) + >>> sorted(robot.group_states["manipulator"].keys()) ['home', 'up'] """ @@ -334,7 +334,7 @@ def get_link_names(self, group=None): Examples -------- - >>> robot.get_link_names('manipulator') + >>> robot.get_link_names("manipulator") ['base_link', 'shoulder_link', 'upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link', 'wrist_3_link', 'ee_link'] """ base_link_name = self.get_base_link_name(group) @@ -377,7 +377,7 @@ def get_configurable_joints(self, group=None): Examples -------- - >>> joints = robot.get_configurable_joints('manipulator') + >>> joints = robot.get_configurable_joints("manipulator") >>> [j.name for j in joints] ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] """ @@ -463,7 +463,7 @@ def get_configurable_joint_types(self, group=None): Examples -------- - >>> robot.get_configurable_joint_types('manipulator') + >>> robot.get_configurable_joint_types("manipulator") [0, 0, 0, 0, 0, 0] """ configurable_joints = self.get_configurable_joints(group) @@ -771,7 +771,7 @@ def to_local_coordinates(self, frame_WCF, group=None): -------- >>> frame_WCF = Frame([-0.363, 0.003, -0.147], [0.388, -0.351, -0.852], [0.276, 0.926, -0.256]) >>> frame_RCF = robot.to_local_coordinates(frame_WCF) - >>> frame_RCF # doctest: +SKIP + >>> frame_RCF # doctest: +SKIP Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256)) # doctest: +SKIP """ frame_RCF = frame_WCF.transformed(self.transformation_WCF_RCF(group)) @@ -796,7 +796,7 @@ def to_world_coordinates(self, frame_RCF, group=None): -------- >>> frame_RCF = Frame([-0.363, 0.003, -0.147], [0.388, -0.351, -0.852], [0.276, 0.926, -0.256]) >>> frame_WCF = robot.to_world_coordinates(frame_RCF) - >>> frame_WCF # doctest: +SKIP + >>> frame_WCF # doctest: +SKIP Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256)) # doctest: +SKIP """ frame_WCF = frame_RCF.transformed(self.transformation_RCF_WCF(group)) @@ -835,13 +835,13 @@ def from_tcf_to_t0cf(self, frames_tcf, group=None): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> robot.attach_tool(Tool(mesh, frame)) >>> frames_tcf = [Frame((-0.309, -0.046, -0.266), (0.276, 0.926, -0.256), (0.879, -0.136, 0.456))] - >>> robot.from_tcf_to_t0cf(frames_tcf) # doctest: +SKIP + >>> robot.from_tcf_to_t0cf(frames_tcf) # doctest: +SKIP [Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256))] # doctest: +SKIP - >>> robot.from_tcf_to_t0cf(frames_tcf, group=robot.main_group_name) # doctest: +SKIP + >>> robot.from_tcf_to_t0cf(frames_tcf, group=robot.main_group_name) # doctest: +SKIP [Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256))] # doctest: +SKIP """ tool = self._get_attached_tool_for_group(group_name=group) @@ -871,13 +871,13 @@ def from_t0cf_to_tcf(self, frames_t0cf, group=None): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> robot.attach_tool(Tool(mesh, frame)) >>> frames_t0cf = [Frame((-0.363, 0.003, -0.147), (0.388, -0.351, -0.852), (0.276, 0.926, -0.256))] - >>> robot.from_t0cf_to_tcf(frames_t0cf) # doctest: +SKIP + >>> robot.from_t0cf_to_tcf(frames_t0cf) # doctest: +SKIP [Frame(Point(-0.309, -0.046, -0.266), Vector(0.276, 0.926, -0.256), Vector(0.879, -0.136, 0.456))] # doctest: +SKIP - >>> robot.from_t0cf_to_tcf(frames_t0cf, group=robot.main_group_name) # doctest: +SKIP + >>> robot.from_t0cf_to_tcf(frames_t0cf, group=robot.main_group_name) # doctest: +SKIP [Frame(Point(-0.309, -0.046, -0.266), Vector(0.276, 0.926, -0.256), Vector(0.879, -0.136, 0.456))] # doctest: +SKIP """ tool = self._get_attached_tool_for_group(group_name=group) @@ -907,7 +907,7 @@ def attach_tool(self, tool, group=None, touch_links=None): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> tool = Tool(mesh, frame) >>> robot.attach_tool(tool) @@ -1085,7 +1085,7 @@ def position_constraint_from_frame(self, frame_WCF, tolerance_position, group=No -------- >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) >>> tolerance_position = 0.001 - >>> robot.position_constraint_from_frame(frame, tolerance_position) # doctest: +SKIP + >>> robot.position_constraint_from_frame(frame, tolerance_position) # doctest: +SKIP PositionConstraint('ee_link', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0) # doctest: +SKIP """ @@ -1146,7 +1146,7 @@ def constraints_from_frame( >>> tolerance_position = 0.001 >>> tolerances_axes = [math.radians(1)] >>> group = robot.main_group_name - >>> robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) # doctest: +SKIP + >>> robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) # doctest: +SKIP [PositionConstraint('ee_link', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0), # doctest: +SKIP OrientationConstraint('ee_link', [0.5, 0.5, 0.5, 0.5], [0.017453292519943295, 0.017453292519943295, 0.017453292519943295], 1.0)] # doctest: +SKIP """ @@ -1301,7 +1301,7 @@ def inverse_kinematics( >>> frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]) >>> start_configuration = robot.zero_configuration() >>> group = robot.main_group_name - >>> robot.inverse_kinematics(frame_WCF, start_configuration, group) # doctest: +SKIP + >>> robot.inverse_kinematics(frame_WCF, start_configuration, group) # doctest: +SKIP Configuration((4.045, 5.130, -2.174, -6.098, -5.616, 6.283), (0, 0, 0, 0, 0, 0)) # doctest: +SKIP """ # Pseudo-memoized sequential calls will re-use iterator if not exhaused @@ -1376,7 +1376,7 @@ def iter_inverse_kinematics( >>> frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]) >>> start_configuration = robot.zero_configuration() >>> group = robot.main_group_name - >>> next(robot.iter_inverse_kinematics(frame_WCF, start_configuration, group)) # doctest: +SKIP + >>> next(robot.iter_inverse_kinematics(frame_WCF, start_configuration, group)) # doctest: +SKIP Configuration((4.045, 5.130, -2.174, -6.098, -5.616, 6.283), (0, 0, 0, 0, 0, 0)) # doctest: +SKIP """ options = options or {} @@ -1497,7 +1497,7 @@ def forward_kinematics(self, configuration, group=None, use_attached_tool_frame= >>> configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.000]) >>> group = robot.main_group_name >>> frame_WCF_c = robot.forward_kinematics(configuration, group) - >>> options = {'solver': 'model'} + >>> options = {"solver": "model"} >>> frame_WCF_m = robot.forward_kinematics(configuration, group, options) >>> frame_WCF_c == frame_WCF_m True @@ -1734,10 +1734,10 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) >>> tolerance_position = 0.001 >>> tolerances_axes = [math.radians(1)] * 3 - >>> start_configuration = Configuration.from_revolute_values([-0.042, 4.295, 0, -3.327, 4.755, 0.]) + >>> start_configuration = Configuration.from_revolute_values([-0.042, 4.295, 0, -3.327, 4.755, 0.0]) >>> group = robot.main_group_name >>> goal_constraints = robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) - >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {'planner_id': 'RRTConnect'}) + >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {"planner_id": "RRTConnect"}) >>> trajectory.fraction 1.0 >>> len(trajectory.points) > 1 @@ -1753,8 +1753,10 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op >>> tolerances_above = [math.radians(5)] * len(configuration.joint_values) >>> tolerances_below = [math.radians(5)] * len(configuration.joint_values) >>> group = robot.main_group_name - >>> goal_constraints = robot.constraints_from_configuration(configuration, tolerances_above, tolerances_below, group) - >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {'planner_id': 'RRTConnect'}) + >>> goal_constraints = robot.constraints_from_configuration( + ... configuration, tolerances_above, tolerances_below, group + ... ) + >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {"planner_id": "RRTConnect"}) >>> trajectory.fraction 1.0 >>> len(trajectory.points) > 1 diff --git a/src/compas_fab/robots/tool.py b/src/compas_fab/robots/tool.py index f3c8e04c83..25b269552c 100644 --- a/src/compas_fab/robots/tool.py +++ b/src/compas_fab/robots/tool.py @@ -30,7 +30,7 @@ class Tool(Data): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> tool = Tool(mesh, frame) @@ -137,12 +137,12 @@ def from_tcf_to_t0cf(self, frames_tcf): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> tool = Tool(mesh, frame) >>> frames_tcf = [Frame((-0.309, -0.046, -0.266), (0.276, 0.926, -0.256), (0.879, -0.136, 0.456))] >>> frames_t0cf = tool.from_tcf_to_t0cf(frames_tcf) - >>> print(frames_t0cf) # doctest: +SKIP + >>> print(frames_t0cf) # doctest: +SKIP [Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256))] # doctest: +SKIP """ return self.tool_model.from_tcf_to_t0cf(frames_tcf) @@ -162,11 +162,11 @@ def from_t0cf_to_tcf(self, frames_t0cf): Examples -------- - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) + >>> mesh = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")) >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) >>> tool = Tool(mesh, frame) >>> frames_t0cf = [Frame((-0.363, 0.003, -0.147), (0.388, -0.351, -0.852), (0.276, 0.926, -0.256))] - >>> tool.from_t0cf_to_tcf(frames_t0cf) # doctest: +SKIP + >>> tool.from_t0cf_to_tcf(frames_t0cf) # doctest: +SKIP [Frame(Point(-0.309, -0.046, -0.266), Vector(0.276, 0.926, -0.256), Vector(0.879, -0.136, 0.456))] # doctest: +SKIP """ return self.tool_model.from_t0cf_to_tcf(frames_t0cf) diff --git a/src/compas_fab/robots/wrench.py b/src/compas_fab/robots/wrench.py index 5128817d20..bdd8f7c4dc 100644 --- a/src/compas_fab/robots/wrench.py +++ b/src/compas_fab/robots/wrench.py @@ -100,9 +100,9 @@ def by_samples(cls, wrenches, proportion_to_cut=0.1): Examples -------- - >>> w1 = Wrench([1, 1, 1], [.1,.1,.1]) - >>> w2 = Wrench([2, 2, 2], [.2,.2,.2]) - >>> w3 = Wrench([3, 3, 3], [.3,.3,.3]) + >>> w1 = Wrench([1, 1, 1], [0.1, 0.1, 0.1]) + >>> w2 = Wrench([2, 2, 2], [0.2, 0.2, 0.2]) + >>> w3 = Wrench([3, 3, 3], [0.3, 0.3, 0.3]) >>> w = Wrench.by_samples([w1, w2, w3]) >>> print(w.force) Vector(x=2.000, y=2.000, z=2.000) diff --git a/src/compas_fab/sensors/base.py b/src/compas_fab/sensors/base.py index 9897abc622..6e8ed330a1 100644 --- a/src/compas_fab/sensors/base.py +++ b/src/compas_fab/sensors/base.py @@ -7,9 +7,9 @@ class SerialSensor(object): port used to communicate with the sensor. Examples: - >>> from serial import Serial # doctest: +SKIP - >>> with Serial('COM5', 57600, timeout=1) as serial: # doctest: +SKIP - ... sensor = SerialSensor(serial) # doctest: +SKIP + >>> from serial import Serial # doctest: +SKIP + >>> with Serial("COM5", 57600, timeout=1) as serial: # doctest: +SKIP + ... sensor = SerialSensor(serial) # doctest: +SKIP """ def __init__(self, serial): diff --git a/src/compas_fab/sensors/baumer.py b/src/compas_fab/sensors/baumer.py index 99c52e6c4c..db8f476b3d 100644 --- a/src/compas_fab/sensors/baumer.py +++ b/src/compas_fab/sensors/baumer.py @@ -33,7 +33,7 @@ class is a context manager type, so it's best used in combination Examples -------- - >>> from serial import Serial # doctest: +SKIP + >>> from serial import Serial # doctest: +SKIP >>> with Serial('COM5', 57600, timeout=1) as serial: # doctest: +SKIP >>> with PosCon3D(serial, PosCon3D.BROADCAST_ADDRESS) as broadcast_query: # doctest: +SKIP ... addr = broadcast_query.get_address() # doctest: +SKIP @@ -360,7 +360,7 @@ class provides access to the serial interface (RS-485). Examples -------- - >>> from serial import Serial # doctest: +SKIP + >>> from serial import Serial # doctest: +SKIP >>> with Serial('COM5', 57600, parity=PARITY_EVEN, timeout=1) as serial: # doctest: +SKIP >>> with PosConCM(serial, PosConCM.BROADCAST_ADDRESS) as broadcast_query: # doctest: +SKIP ... addr = broadcast_query.get_address() # doctest: +SKIP From 06d51fee9423d686030ac858a0fdfb27a6d6d2c0 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 18:05:53 +0100 Subject: [PATCH 23/36] removed deprecated linting and formatting tools from requirements --- requirements-dev.txt | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/requirements-dev.txt b/requirements-dev.txt index f1bd04139d..f1e993468b 100644 --- a/requirements-dev.txt +++ b/requirements-dev.txt @@ -1,20 +1,16 @@ attrs >=19.3.0 -autopep8 black bump2version >=1.0.1 check-manifest >=0.36 compas-invocations2 -doc8 -flake8 ruff importlib_metadata <5.0 invoke>=0.14 -isort -pylint pytest pytest_mock pytest-cov sphinx_compas2_theme sybil twine +tomlkit -e . From bc496f18303cedd3d2b9925118860fa96ba1f472 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Mon, 24 Mar 2025 18:06:05 +0100 Subject: [PATCH 24/36] updated integratino test --- .github/workflows/integration.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/integration.yml b/.github/workflows/integration.yml index c3642d853a..e1c5e005e0 100644 --- a/.github/workflows/integration.yml +++ b/.github/workflows/integration.yml @@ -12,17 +12,17 @@ on: jobs: build: - name: ubuntu-py38-integration + name: ubuntu-py311-integration runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 - - name: Set up Python 3.8 + - name: Set up Python 3.11 uses: actions/setup-python@v2 with: - python-version: 3.8 + python-version: '3.11' - name: Set up docker containers run: | - docker-compose -f "tests/integration_setup/docker-compose.yml" up -d --build + docker compose -f "tests/integration_setup/docker-compose.yml" up -d --build docker ps -a - name: Install dependencies run: | From 2f61c0bf8d78c3422fa06643c5fda4cbc96c6264 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Tue, 25 Mar 2025 09:49:20 +0100 Subject: [PATCH 25/36] pinned sybil version to fix integration test --- requirements-dev.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements-dev.txt b/requirements-dev.txt index f1e993468b..9150706453 100644 --- a/requirements-dev.txt +++ b/requirements-dev.txt @@ -10,7 +10,7 @@ pytest pytest_mock pytest-cov sphinx_compas2_theme -sybil +sybil~=8.0.1 twine tomlkit -e . From cd92dc6580007ceeda2c02fcb23ad128b6fa338d Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Wed, 26 Mar 2025 11:11:55 +0100 Subject: [PATCH 26/36] import create_id from compas_ghpython and remove local implementation --- .../ghpython/components/Cf_PlanCartesianMotion/code.py | 3 +-- src/compas_fab/ghpython/components/Cf_PlanMotion/code.py | 3 +-- src/compas_fab/ghpython/components/Cf_PlanningScene/code.py | 2 +- src/compas_fab/ghpython/components/Cf_RosConnect/code.py | 2 +- src/compas_fab/ghpython/components/Cf_RosRobot/code.py | 3 +-- src/compas_fab/ghpython/components/Cf_RosTopicPublish/code.py | 2 +- .../ghpython/components/Cf_RosTopicSubscribe/code.py | 2 +- src/compas_fab/ghpython/components/__init__.py | 4 ---- .../components_cpython/Cf_PlanCartesianMotion/code.py | 3 +-- .../ghpython/components_cpython/Cf_PlanMotion/code.py | 3 +-- .../ghpython/components_cpython/Cf_PlanningScene/code.py | 2 +- .../ghpython/components_cpython/Cf_RosConnect/code.py | 2 +- .../ghpython/components_cpython/Cf_RosRobot/code.py | 3 +-- .../ghpython/components_cpython/Cf_RosTopicPublish/code.py | 2 +- .../ghpython/components_cpython/Cf_RosTopicSubscribe/code.py | 2 +- 15 files changed, 14 insertions(+), 24 deletions(-) diff --git a/src/compas_fab/ghpython/components/Cf_PlanCartesianMotion/code.py b/src/compas_fab/ghpython/components/Cf_PlanCartesianMotion/code.py index 305f4c3820..36484bc6f6 100644 --- a/src/compas_fab/ghpython/components/Cf_PlanCartesianMotion/code.py +++ b/src/compas_fab/ghpython/components/Cf_PlanCartesianMotion/code.py @@ -4,12 +4,11 @@ COMPAS FAB v1.0.2 """ +from compas_ghpython import create_id from compas_rhino.conversions import plane_to_compas_frame from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st -from compas_fab.ghpython.components import create_id - class PlanCartesianMotion(component): def RunScript( diff --git a/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py b/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py index c25f0fd255..cc2033f4c6 100644 --- a/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py +++ b/src/compas_fab/ghpython/components/Cf_PlanMotion/code.py @@ -4,11 +4,10 @@ COMPAS FAB v1.0.2 """ +from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st -from compas_fab.ghpython.components import create_id - class PlanMotion(component): def RunScript( diff --git a/src/compas_fab/ghpython/components/Cf_PlanningScene/code.py b/src/compas_fab/ghpython/components/Cf_PlanningScene/code.py index cd37c2ad55..dc5b02be25 100644 --- a/src/compas_fab/ghpython/components/Cf_PlanningScene/code.py +++ b/src/compas_fab/ghpython/components/Cf_PlanningScene/code.py @@ -4,10 +4,10 @@ COMPAS FAB v1.0.2 """ +from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st -from compas_fab.ghpython.components import create_id from compas_fab.robots import PlanningScene diff --git a/src/compas_fab/ghpython/components/Cf_RosConnect/code.py b/src/compas_fab/ghpython/components/Cf_RosConnect/code.py index 94bf44ff39..eae8c59fd0 100644 --- a/src/compas_fab/ghpython/components/Cf_RosConnect/code.py +++ b/src/compas_fab/ghpython/components/Cf_RosConnect/code.py @@ -4,11 +4,11 @@ COMPAS FAB v1.0.2 """ +from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st from compas_fab.backends import RosClient -from compas_fab.ghpython.components import create_id class ROSConnect(component): diff --git a/src/compas_fab/ghpython/components/Cf_RosRobot/code.py b/src/compas_fab/ghpython/components/Cf_RosRobot/code.py index 5aad8e3136..b24bbab3d5 100644 --- a/src/compas_fab/ghpython/components/Cf_RosRobot/code.py +++ b/src/compas_fab/ghpython/components/Cf_RosRobot/code.py @@ -5,11 +5,10 @@ """ from compas.scene import SceneObject +from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st -from compas_fab.ghpython.components import create_id - class ROSRobot(component): def RunScript(self, ros_client, load): diff --git a/src/compas_fab/ghpython/components/Cf_RosTopicPublish/code.py b/src/compas_fab/ghpython/components/Cf_RosTopicPublish/code.py index 2684c9252e..ca8c009e11 100644 --- a/src/compas_fab/ghpython/components/Cf_RosTopicPublish/code.py +++ b/src/compas_fab/ghpython/components/Cf_RosTopicPublish/code.py @@ -6,12 +6,12 @@ import time +from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from roslibpy import Topic from scriptcontext import sticky as st from compas_fab.backends.ros.messages import ROSmsg -from compas_fab.ghpython.components import create_id class ROSTopicPublish(component): diff --git a/src/compas_fab/ghpython/components/Cf_RosTopicSubscribe/code.py b/src/compas_fab/ghpython/components/Cf_RosTopicSubscribe/code.py index 7efec6c02d..ba1c2b74cb 100644 --- a/src/compas_fab/ghpython/components/Cf_RosTopicSubscribe/code.py +++ b/src/compas_fab/ghpython/components/Cf_RosTopicSubscribe/code.py @@ -7,12 +7,12 @@ import time import Grasshopper.Kernel +from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from roslibpy import Topic from scriptcontext import sticky as st from compas_fab.backends.ros.messages import ROSmsg -from compas_fab.ghpython.components import create_id class ROSTopicSubscribe(component): diff --git a/src/compas_fab/ghpython/components/__init__.py b/src/compas_fab/ghpython/components/__init__.py index 839ed05b2f..07eb542b82 100644 --- a/src/compas_fab/ghpython/components/__init__.py +++ b/src/compas_fab/ghpython/components/__init__.py @@ -6,10 +6,6 @@ pass -def create_id(component, name): - return "{}_{}".format(name, component.InstanceGuid) - - def warning(component, message): """Add a warning message to the component. diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py index 61255c037c..0d74422591 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanCartesianMotion/code.py @@ -7,11 +7,10 @@ import Grasshopper import System +from compas_ghpython import create_id from compas_rhino.conversions import plane_to_compas_frame from scriptcontext import sticky as st -from compas_fab.ghpython.components import create_id - class PlanCartesianMotion(Grasshopper.Kernel.GH_ScriptInstance): def RunScript( diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py index 1ef5a51a68..35fb910586 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanMotion/code.py @@ -7,10 +7,9 @@ import Grasshopper import System +from compas_ghpython import create_id from scriptcontext import sticky as st -from compas_fab.ghpython.components import create_id - class PlanMotion(Grasshopper.Kernel.GH_ScriptInstance): def RunScript( diff --git a/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py index 227d7e690f..752d75d987 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_PlanningScene/code.py @@ -6,9 +6,9 @@ """ import Grasshopper +from compas_ghpython import create_id from scriptcontext import sticky as st -from compas_fab.ghpython.components import create_id from compas_fab.robots import PlanningScene diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py index e970bb63ba..c907aa8879 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosConnect/code.py @@ -6,10 +6,10 @@ """ import Grasshopper +from compas_ghpython import create_id from scriptcontext import sticky as st from compas_fab.backends import RosClient -from compas_fab.ghpython.components import create_id class ROSConnect(Grasshopper.Kernel.GH_ScriptInstance): diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py index 1c0cd8ce80..cb5d5f649b 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py @@ -7,10 +7,9 @@ import Grasshopper from compas.scene import SceneObject +from compas_ghpython import create_id from scriptcontext import sticky as st -from compas_fab.ghpython.components import create_id - class ROSRobot(Grasshopper.Kernel.GH_ScriptInstance): def RunScript(self, ros_client, load): diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py index 78880ff8b2..eb588b31b5 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicPublish/code.py @@ -8,11 +8,11 @@ import time import Grasshopper +from compas_ghpython import create_id from roslibpy import Topic from scriptcontext import sticky as st from compas_fab.backends.ros.messages import ROSmsg -from compas_fab.ghpython.components import create_id from compas_fab.ghpython.components import message from compas_fab.ghpython.components import warning diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py index e491df60cb..d935ccf00d 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosTopicSubscribe/code.py @@ -8,11 +8,11 @@ import time import Grasshopper +from compas_ghpython import create_id from roslibpy import Topic from scriptcontext import sticky as st from compas_fab.backends.ros.messages import ROSmsg -from compas_fab.ghpython.components import create_id from compas_fab.ghpython.components import message from compas_fab.ghpython.components import warning From fbc7bdbf40515f343dca0819817eb1ae7d12dfb9 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Wed, 26 Mar 2025 11:14:24 +0100 Subject: [PATCH 27/36] updated changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7922926f79..8d0d215a7e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Removed +* Removed `create_id` from `compas_fab.ghpython.components`, using `compas_ghpython.create_id` instead. + ## [1.0.2] 2024-02-22 From 8392c8b0d80d53b672c1e0f956003f1a0b700ad2 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Thu, 27 Mar 2025 12:02:11 +0100 Subject: [PATCH 28/36] fixed scene object not properly created --- CHANGELOG.md | 1 + src/compas_fab/ghpython/components/Cf_RosRobot/code.py | 2 +- src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8d0d215a7e..a182547c35 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 * Updated dev dependency to `compas_invocations2`. * Fixed `AttributeError` in `inverse_kinematics_spherical_wrist()`. +* Fixed `AttributeError` in VisualizeRobot GH component. ### Removed diff --git a/src/compas_fab/ghpython/components/Cf_RosRobot/code.py b/src/compas_fab/ghpython/components/Cf_RosRobot/code.py index b24bbab3d5..810be2de69 100644 --- a/src/compas_fab/ghpython/components/Cf_RosRobot/code.py +++ b/src/compas_fab/ghpython/components/Cf_RosRobot/code.py @@ -17,7 +17,7 @@ def RunScript(self, ros_client, load): if ros_client and ros_client.is_connected and load: # Load URDF from ROS st[key] = ros_client.load_robot(load_geometry=True, precision=12) - st[key].scene_object = SceneObject(st[key].model) + st[key].scene_object = SceneObject(item=st[key].model) robot = st.get(key, None) if robot: # client sometimes need to be restarted, without needing to reload geometry diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py index cb5d5f649b..2f986ef8be 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py @@ -18,7 +18,7 @@ def RunScript(self, ros_client, load): if ros_client and ros_client.is_connected and load: # Load URDF from ROS st[key] = ros_client.load_robot(load_geometry=True, precision=12) - st[key].scene_object = SceneObject(st[key].model) + st[key].scene_object = SceneObject(item=st[key].model) robot = st.get(key, None) if robot: # client sometimes need to be restarted, without needing to reload geometry From 51afadd308b146a95a50eacfe09838a5d5e710e2 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Thu, 27 Mar 2025 12:05:49 +0100 Subject: [PATCH 29/36] same SceneObject fix in a couple other places --- .../ghpython/components/Cf_VisualizeRobot/code.py | 8 +++----- .../ghpython/components_cpython/Cf_VisualizeRobot/code.py | 8 +++----- src/compas_fab/ghpython/reachabilitymapobject.py | 6 +++--- src/compas_fab/rhino/reachabilitymapobject.py | 6 +++--- 4 files changed, 12 insertions(+), 16 deletions(-) diff --git a/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py b/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py index 143c75ff09..5d39c65b0e 100644 --- a/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py +++ b/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py @@ -93,9 +93,7 @@ def RunScript( try: scene = robot.client.get_planning_scene() except BackendFeatureNotSupportedError: - print( - "The selected backend does not support collision meshes. If you need collision mesh support, use a different backend." - ) + print("The selected backend does not support collision meshes. If you need collision mesh support, use a different backend.") scene = None show_cm = False show_acm = False @@ -115,7 +113,7 @@ def RunScript( else: mesh = cm.mesh - collision_meshes.extend(SceneObject(mesh).draw()) + collision_meshes.extend(SceneObject(item=mesh).draw()) cached_scene["cm"] = collision_meshes @@ -136,7 +134,7 @@ def RunScript( mesh = acm.collision_mesh.mesh.transformed(t) - attached_meshes.extend(SceneObject(mesh).draw()) + attached_meshes.extend(SceneObject(item=mesh).draw()) cached_scene["acm"] = attached_meshes diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py index 718727c977..5a1fe370ae 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py @@ -94,9 +94,7 @@ def RunScript( try: scene = robot.client.get_planning_scene() except BackendFeatureNotSupportedError: - print( - "The selected backend does not support collision meshes. If you need collision mesh support, use a different backend." - ) + print("The selected backend does not support collision meshes. If you need collision mesh support, use a different backend.") scene = None show_cm = False show_acm = False @@ -116,7 +114,7 @@ def RunScript( else: mesh = cm.mesh - collision_meshes.extend(SceneObject(mesh).draw()) + collision_meshes.extend(SceneObject(item=mesh).draw()) cached_scene["cm"] = collision_meshes @@ -137,7 +135,7 @@ def RunScript( mesh = acm.collision_mesh.mesh.transformed(t) - attached_meshes.extend(SceneObject(mesh).draw()) + attached_meshes.extend(SceneObject(item=mesh).draw()) cached_scene["acm"] = attached_meshes diff --git a/src/compas_fab/ghpython/reachabilitymapobject.py b/src/compas_fab/ghpython/reachabilitymapobject.py index 58f7dd0f4a..eeaf9c1550 100644 --- a/src/compas_fab/ghpython/reachabilitymapobject.py +++ b/src/compas_fab/ghpython/reachabilitymapobject.py @@ -34,12 +34,12 @@ def draw_frames(self, ik_index=None): for frames in self.reachability_map.frames: xframes.append([]) for frame in frames: - xframes[-1].append(SceneObject(frame).draw()) + xframes[-1].append(SceneObject(item=frame).draw()) xframes = list_to_tree(xframes) return xframes else: frames, _ = self.reachability_map.reachable_frames_and_configurations_at_ik_index(ik_index) - return [SceneObject(f).draw() for f in frames] + return [SceneObject(item=f).draw() for f in frames] def draw(self, colormap="viridis"): return self.draw_cloud(colormap) @@ -64,7 +64,7 @@ def draw_cloud(self, colormap="viridis", points=None): xpoints = [] for pt in points: - xpoints.extend(SceneObject(pt).draw()) + xpoints.extend(SceneObject(item=pt).draw()) colors = [] cmap = ColorMap.from_mpl(colormap) diff --git a/src/compas_fab/rhino/reachabilitymapobject.py b/src/compas_fab/rhino/reachabilitymapobject.py index fdb87994b7..3f0c83d805 100644 --- a/src/compas_fab/rhino/reachabilitymapobject.py +++ b/src/compas_fab/rhino/reachabilitymapobject.py @@ -43,12 +43,12 @@ def draw_frames(self, ik_index=None): for frames in self.reachability_map.frames: xframes.append([]) for frame in frames: - xframes[-1].extend(SceneObject(frame, layer=self.layer, scale=self.scale).draw()) + xframes[-1].extend(SceneObject(item=frame, layer=self.layer, scale=self.scale).draw()) return xframes else: frames, _ = self.reachability_map.reachable_frames_and_configurations_at_ik_index(ik_index) - return [SceneObject(f, layer=self.layer, scale=self.scale).draw() for f in frames] + return [SceneObject(item=f, layer=self.layer, scale=self.scale).draw() for f in frames] def draw(self, colormap="viridis"): return self.draw_cloud(colormap) @@ -82,7 +82,7 @@ def draw_cloud(self, colormap="viridis", points=None): guids = [] for num, pt in zip(score, points): color = cmap(num, minv, maxv) - sceneobject = SceneObject(pt, layer=self.layer) + sceneobject = SceneObject(item=pt, layer=self.layer) guids.extend(sceneobject.draw(color)) return guids From 1e9064472dc88e1716e8a308efa948acb4da06e9 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Fri, 28 Mar 2025 15:55:25 +0100 Subject: [PATCH 30/36] untar packages which cannot be installed with ironpip --- .github/workflows/ironpython.yml | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ironpython.yml b/.github/workflows/ironpython.yml index 0bd4d17003..198361c5ae 100644 --- a/.github/workflows/ironpython.yml +++ b/.github/workflows/ironpython.yml @@ -28,6 +28,11 @@ jobs: ipy -X:Frames -m pip install --no-deps compas.tar.gz ipy -X:Frames -m pip install --no-deps roslibpy.tar.gz ipy -X:Frames -m pip install --no-deps compas_robots.tar.gz + + # untar and rename, these cannot be installed using ironpip because they not longer have a setup.py + tar -xf compas.tar.gz && for /d %i in (compas-*) do ren "%i" compas + tar -xf compas_robots.tar.gz && for /d %i in (compas_robots-*) do ren "%i" compas_robots + - uses: NuGet/setup-nuget@v1.0.5 - uses: compas-dev/compas-actions.ghpython_components@v5 with: @@ -37,9 +42,9 @@ jobs: run: | ipy -m compas_fab env: - IRONPYTHONPATH: ./src + IRONPYTHONPATH: ./src;./compas/src;./compas_robots/src - name: Run tests run: | ipy tests/ipy_test_runner.py env: - IRONPYTHONPATH: ./src + IRONPYTHONPATH: ./src;./compas/src;./compas_robots/src From fd7e005777fc5a5da78b8907c9affbf2fb23dfaf Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Fri, 28 Mar 2025 16:10:59 +0100 Subject: [PATCH 31/36] linting --- src/compas_fab/ghpython/components/Cf_RosRobot/code.py | 3 ++- .../ghpython/components/Cf_VisualizeRobot/code.py | 10 ++++++---- .../ghpython/components_cpython/Cf_RosRobot/code.py | 3 ++- .../components_cpython/Cf_VisualizeRobot/code.py | 10 ++++++---- src/compas_fab/ghpython/reachabilitymapobject.py | 3 ++- src/compas_fab/rhino/reachabilitymapobject.py | 3 ++- 6 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/compas_fab/ghpython/components/Cf_RosRobot/code.py b/src/compas_fab/ghpython/components/Cf_RosRobot/code.py index 810be2de69..6472fd0bbb 100644 --- a/src/compas_fab/ghpython/components/Cf_RosRobot/code.py +++ b/src/compas_fab/ghpython/components/Cf_RosRobot/code.py @@ -4,11 +4,12 @@ COMPAS FAB v1.0.2 """ -from compas.scene import SceneObject from compas_ghpython import create_id from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st +from compas.scene import SceneObject + class ROSRobot(component): def RunScript(self, ros_client, load): diff --git a/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py b/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py index 5d39c65b0e..100322be1c 100644 --- a/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py +++ b/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py @@ -6,14 +6,14 @@ import time -from compas.geometry import Frame -from compas.geometry import Transformation -from compas.scene import SceneObject from compas_ghpython import create_id from compas_rhino.conversions import frame_to_rhino_plane from ghpythonlib.componentbase import executingcomponent as component from scriptcontext import sticky as st +from compas.geometry import Frame +from compas.geometry import Transformation +from compas.scene import SceneObject from compas_fab.backends import BackendFeatureNotSupportedError from compas_fab.robots import PlanningScene @@ -93,7 +93,9 @@ def RunScript( try: scene = robot.client.get_planning_scene() except BackendFeatureNotSupportedError: - print("The selected backend does not support collision meshes. If you need collision mesh support, use a different backend.") + print( + "The selected backend does not support collision meshes. If you need collision mesh support, use a different backend." + ) scene = None show_cm = False show_acm = False diff --git a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py index 2f986ef8be..b90d0f139c 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_RosRobot/code.py @@ -6,10 +6,11 @@ """ import Grasshopper -from compas.scene import SceneObject from compas_ghpython import create_id from scriptcontext import sticky as st +from compas.scene import SceneObject + class ROSRobot(Grasshopper.Kernel.GH_ScriptInstance): def RunScript(self, ros_client, load): diff --git a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py index 5a1fe370ae..8bb01fbc59 100644 --- a/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py +++ b/src/compas_fab/ghpython/components_cpython/Cf_VisualizeRobot/code.py @@ -8,13 +8,13 @@ import time import Grasshopper -from compas.geometry import Frame -from compas.geometry import Transformation -from compas.scene import SceneObject from compas_ghpython import create_id from compas_rhino.conversions import frame_to_rhino_plane from scriptcontext import sticky as st +from compas.geometry import Frame +from compas.geometry import Transformation +from compas.scene import SceneObject from compas_fab.backends import BackendFeatureNotSupportedError from compas_fab.robots import PlanningScene @@ -94,7 +94,9 @@ def RunScript( try: scene = robot.client.get_planning_scene() except BackendFeatureNotSupportedError: - print("The selected backend does not support collision meshes. If you need collision mesh support, use a different backend.") + print( + "The selected backend does not support collision meshes. If you need collision mesh support, use a different backend." + ) scene = None show_cm = False show_acm = False diff --git a/src/compas_fab/ghpython/reachabilitymapobject.py b/src/compas_fab/ghpython/reachabilitymapobject.py index eeaf9c1550..7f3e9320e7 100644 --- a/src/compas_fab/ghpython/reachabilitymapobject.py +++ b/src/compas_fab/ghpython/reachabilitymapobject.py @@ -1,6 +1,7 @@ +from compas_ghpython.scene import GHSceneObject + from compas.colors import ColorMap from compas.scene import SceneObject -from compas_ghpython.scene import GHSceneObject class ReachabilityMapObject(GHSceneObject): diff --git a/src/compas_fab/rhino/reachabilitymapobject.py b/src/compas_fab/rhino/reachabilitymapobject.py index 3f0c83d805..a39d1fe715 100644 --- a/src/compas_fab/rhino/reachabilitymapobject.py +++ b/src/compas_fab/rhino/reachabilitymapobject.py @@ -1,6 +1,7 @@ +from compas_rhino.scene import RhinoSceneObject + from compas.colors import ColorMap from compas.scene import SceneObject -from compas_rhino.scene import RhinoSceneObject class ReachabilityMapObject(RhinoSceneObject): From fb3d9b775c5e2dc6ec13ff776c5a630c605537cd Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Fri, 28 Mar 2025 16:14:37 +0100 Subject: [PATCH 32/36] ironpython test --- tests/robots/test_robot.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/tests/robots/test_robot.py b/tests/robots/test_robot.py index c30cc9f9c0..815407dc9c 100644 --- a/tests/robots/test_robot.py +++ b/tests/robots/test_robot.py @@ -3,6 +3,7 @@ import re import pytest +import compas from compas.data import json_dumps from compas.data import json_loads from compas.datastructures import Mesh @@ -471,11 +472,13 @@ def test_wrong_group_name_raises_exception(ur5_robot_instance, robot_tool1): robot.attach_tool(tool, group=wrong_group_name) -def test_attached_tools_no_assigning(ur5_robot_instance, robot_tool1): - robot = ur5_robot_instance +if not compas.IPY: + # this test causes a stack overflow in IronPython for some reason I couldn't track down + def test_attached_tools_no_assigning(ur5_robot_instance, robot_tool1): + robot = ur5_robot_instance - with pytest.raises(AttributeError): - robot.attached_tools = None + with pytest.raises(AttributeError): + robot.attached_tools = None def test_print_robot_info(ur5_robot_instance): From 9f442e95d37ddff58e523ddfec272e6b9c975947 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Fri, 28 Mar 2025 16:18:29 +0100 Subject: [PATCH 33/36] skip ros integration tests --- src/compas_fab/robots/robot.py | 62 ++++++++-------------------------- 1 file changed, 15 insertions(+), 47 deletions(-) diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 940ab309fa..e57a57ffd7 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -549,9 +549,7 @@ def get_group_configuration(self, group, full_configuration): :class:`Configuration` The configuration of the group. """ - full_configuration = self._check_full_configuration_and_scale(full_configuration)[ - 0 - ] # adds joint_names to full_configuration and makes copy + full_configuration = self._check_full_configuration_and_scale(full_configuration)[0] # adds joint_names to full_configuration and makes copy group_joint_names = self.get_configurable_joint_names(group) values = [full_configuration[name] for name in group_joint_names] return Configuration(values, self.get_configurable_joint_types(group), group_joint_names) @@ -585,9 +583,7 @@ def merge_group_with_full_configuration(self, group_configuration, full_configur if not group_configuration.joint_names: group_configuration.joint_names = self.get_configurable_joint_names(group) - full_configuration = self._check_full_configuration_and_scale(full_configuration)[ - 0 - ] # adds joint_names to full_configuration and makes copy + full_configuration = self._check_full_configuration_and_scale(full_configuration)[0] # adds joint_names to full_configuration and makes copy full_configuration = full_configuration.merged(group_configuration) return full_configuration @@ -658,11 +654,7 @@ def _check_full_configuration_and_scale(self, full_configuration=None): joint_names = self.get_configurable_joint_names() # full configuration # full_configuration might have passive joints specified as well, we allow this. if len(joint_names) > len(full_configuration.joint_values): - raise ValueError( - "Please pass a configuration with {} values, for all configurable joints of the robot.".format( - len(joint_names) - ) - ) + raise ValueError("Please pass a configuration with {} values, for all configurable joints of the robot.".format(len(joint_names))) configuration = full_configuration.copy() if not configuration.joint_names: configuration.joint_names = joint_names @@ -1097,9 +1089,7 @@ def position_constraint_from_frame(self, frame_WCF, tolerance_position, group=No sphere = Sphere(radius=tolerance_position, point=frame_WCF.point) return PositionConstraint.from_sphere(ee_link, sphere) - def constraints_from_frame( - self, frame_WCF, tolerance_position, tolerances_axes, group=None, use_attached_tool_frame=True - ): + def constraints_from_frame(self, frame_WCF, tolerance_position, tolerances_axes, group=None, use_attached_tool_frame=True): r"""Create a position and an orientation constraint from a frame calculated for the group's end-effector link. Parameters @@ -1211,32 +1201,18 @@ def constraints_from_configuration(self, configuration, tolerances_above, tolera joint_names = self.get_configurable_joint_names(group) if len(joint_names) != len(configuration.joint_values): - raise ValueError( - "The passed configuration has {} joint_values, the group {} needs however: {}".format( - len(configuration.joint_values), group, len(joint_names) - ) - ) + raise ValueError("The passed configuration has {} joint_values, the group {} needs however: {}".format(len(configuration.joint_values), group, len(joint_names))) if len(tolerances_above) == 1: tolerances_above = tolerances_above * len(joint_names) elif len(tolerances_above) != len(configuration.joint_values): - raise ValueError( - "The passed configuration has {} joint_values, the tolerances_above however: {}".format( - len(configuration.joint_values), len(tolerances_above) - ) - ) + raise ValueError("The passed configuration has {} joint_values, the tolerances_above however: {}".format(len(configuration.joint_values), len(tolerances_above))) if len(tolerances_below) == 1: tolerances_below = tolerances_below * len(joint_names) elif len(tolerances_below) != len(configuration.joint_values): - raise ValueError( - "The passed configuration has {} joint_values, the tolerances_below however: {}".format( - len(configuration.joint_values), len(tolerances_below) - ) - ) + raise ValueError("The passed configuration has {} joint_values, the tolerances_below however: {}".format(len(configuration.joint_values), len(tolerances_below))) constraints = [] - for name, value, tolerance_above, tolerance_below in zip( - joint_names, configuration.joint_values, tolerances_above, tolerances_below - ): + for name, value, tolerance_above, tolerance_below in zip(joint_names, configuration.joint_values, tolerances_above, tolerances_below): constraints.append(JointConstraint(name, value, tolerance_above, tolerance_below)) return constraints @@ -1305,18 +1281,14 @@ def inverse_kinematics( Configuration((4.045, 5.130, -2.174, -6.098, -5.616, 6.283), (0, 0, 0, 0, 0, 0)) # doctest: +SKIP """ # Pseudo-memoized sequential calls will re-use iterator if not exhaused - request_id = "{}-{}-{}-{}-{}".format( - str(frame_WCF), str(start_configuration), str(group), str(return_full_configuration), str(options) - ) + request_id = "{}-{}-{}-{}-{}".format(str(frame_WCF), str(start_configuration), str(group), str(return_full_configuration), str(options)) if self._current_ik["request_id"] == request_id and self._current_ik["solutions"] is not None: solution = next(self._current_ik["solutions"], None) if solution is not None: return solution - solutions = self.iter_inverse_kinematics( - frame_WCF, start_configuration, group, return_full_configuration, use_attached_tool_frame, options - ) + solutions = self.iter_inverse_kinematics(frame_WCF, start_configuration, group, return_full_configuration, use_attached_tool_frame, options) self._current_ik["request_id"] = request_id self._current_ik["solutions"] = solutions @@ -1540,9 +1512,7 @@ def forward_kinematics(self, configuration, group=None, use_attached_tool_frame= def forward_kinematics_deprecated(self, configuration, group=None, backend=None, ee_link=None): return self.forward_kinematics(configuration, group, options=dict(solver=backend, link=ee_link)) - def plan_cartesian_motion( - self, frames_WCF, start_configuration=None, group=None, use_attached_tool_frame=True, options=None - ): + def plan_cartesian_motion(self, frames_WCF, start_configuration=None, group=None, use_attached_tool_frame=True, options=None): """Calculate a cartesian motion path (linear in tool space). Parameters @@ -1584,7 +1554,7 @@ def plan_cartesian_motion( Examples -------- - >>> ros = RosClient() + >>> ros = RosClient() # doctest: +SKIP >>> ros.run() >>> robot = ros.load_robot() >>> frames = [Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]),\ @@ -1728,7 +1698,7 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op Using position and orientation constraints: - >>> ros = RosClient() + >>> ros = RosClient() # doctest: +SKIP >>> ros.run() >>> robot = ros.load_robot() >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) @@ -1746,16 +1716,14 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op Using joint constraints (to the UP configuration): - >>> ros = RosClient() + >>> ros = RosClient() # doctest: +SKIP >>> ros.run() >>> robot = ros.load_robot() >>> configuration = Configuration.from_revolute_values([0.0, -1.5707, 0.0, -1.5707, 0.0, 0.0]) >>> tolerances_above = [math.radians(5)] * len(configuration.joint_values) >>> tolerances_below = [math.radians(5)] * len(configuration.joint_values) >>> group = robot.main_group_name - >>> goal_constraints = robot.constraints_from_configuration( - ... configuration, tolerances_above, tolerances_below, group - ... ) + >>> goal_constraints = robot.constraints_from_configuration(configuration, tolerances_above, tolerances_below, group) >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {"planner_id": "RRTConnect"}) >>> trajectory.fraction 1.0 From af8f1f6b66d4ebd72ea38dbb7aedf90c2f332f77 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Fri, 28 Mar 2025 16:20:47 +0100 Subject: [PATCH 34/36] formatting --- src/compas_fab/robots/robot.py | 56 ++++++++++++++++++++++++++-------- 1 file changed, 44 insertions(+), 12 deletions(-) diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index e57a57ffd7..50424ef8a9 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -549,7 +549,9 @@ def get_group_configuration(self, group, full_configuration): :class:`Configuration` The configuration of the group. """ - full_configuration = self._check_full_configuration_and_scale(full_configuration)[0] # adds joint_names to full_configuration and makes copy + full_configuration = self._check_full_configuration_and_scale(full_configuration)[ + 0 + ] # adds joint_names to full_configuration and makes copy group_joint_names = self.get_configurable_joint_names(group) values = [full_configuration[name] for name in group_joint_names] return Configuration(values, self.get_configurable_joint_types(group), group_joint_names) @@ -583,7 +585,9 @@ def merge_group_with_full_configuration(self, group_configuration, full_configur if not group_configuration.joint_names: group_configuration.joint_names = self.get_configurable_joint_names(group) - full_configuration = self._check_full_configuration_and_scale(full_configuration)[0] # adds joint_names to full_configuration and makes copy + full_configuration = self._check_full_configuration_and_scale(full_configuration)[ + 0 + ] # adds joint_names to full_configuration and makes copy full_configuration = full_configuration.merged(group_configuration) return full_configuration @@ -654,7 +658,11 @@ def _check_full_configuration_and_scale(self, full_configuration=None): joint_names = self.get_configurable_joint_names() # full configuration # full_configuration might have passive joints specified as well, we allow this. if len(joint_names) > len(full_configuration.joint_values): - raise ValueError("Please pass a configuration with {} values, for all configurable joints of the robot.".format(len(joint_names))) + raise ValueError( + "Please pass a configuration with {} values, for all configurable joints of the robot.".format( + len(joint_names) + ) + ) configuration = full_configuration.copy() if not configuration.joint_names: configuration.joint_names = joint_names @@ -1089,7 +1097,9 @@ def position_constraint_from_frame(self, frame_WCF, tolerance_position, group=No sphere = Sphere(radius=tolerance_position, point=frame_WCF.point) return PositionConstraint.from_sphere(ee_link, sphere) - def constraints_from_frame(self, frame_WCF, tolerance_position, tolerances_axes, group=None, use_attached_tool_frame=True): + def constraints_from_frame( + self, frame_WCF, tolerance_position, tolerances_axes, group=None, use_attached_tool_frame=True + ): r"""Create a position and an orientation constraint from a frame calculated for the group's end-effector link. Parameters @@ -1201,18 +1211,32 @@ def constraints_from_configuration(self, configuration, tolerances_above, tolera joint_names = self.get_configurable_joint_names(group) if len(joint_names) != len(configuration.joint_values): - raise ValueError("The passed configuration has {} joint_values, the group {} needs however: {}".format(len(configuration.joint_values), group, len(joint_names))) + raise ValueError( + "The passed configuration has {} joint_values, the group {} needs however: {}".format( + len(configuration.joint_values), group, len(joint_names) + ) + ) if len(tolerances_above) == 1: tolerances_above = tolerances_above * len(joint_names) elif len(tolerances_above) != len(configuration.joint_values): - raise ValueError("The passed configuration has {} joint_values, the tolerances_above however: {}".format(len(configuration.joint_values), len(tolerances_above))) + raise ValueError( + "The passed configuration has {} joint_values, the tolerances_above however: {}".format( + len(configuration.joint_values), len(tolerances_above) + ) + ) if len(tolerances_below) == 1: tolerances_below = tolerances_below * len(joint_names) elif len(tolerances_below) != len(configuration.joint_values): - raise ValueError("The passed configuration has {} joint_values, the tolerances_below however: {}".format(len(configuration.joint_values), len(tolerances_below))) + raise ValueError( + "The passed configuration has {} joint_values, the tolerances_below however: {}".format( + len(configuration.joint_values), len(tolerances_below) + ) + ) constraints = [] - for name, value, tolerance_above, tolerance_below in zip(joint_names, configuration.joint_values, tolerances_above, tolerances_below): + for name, value, tolerance_above, tolerance_below in zip( + joint_names, configuration.joint_values, tolerances_above, tolerances_below + ): constraints.append(JointConstraint(name, value, tolerance_above, tolerance_below)) return constraints @@ -1281,14 +1305,18 @@ def inverse_kinematics( Configuration((4.045, 5.130, -2.174, -6.098, -5.616, 6.283), (0, 0, 0, 0, 0, 0)) # doctest: +SKIP """ # Pseudo-memoized sequential calls will re-use iterator if not exhaused - request_id = "{}-{}-{}-{}-{}".format(str(frame_WCF), str(start_configuration), str(group), str(return_full_configuration), str(options)) + request_id = "{}-{}-{}-{}-{}".format( + str(frame_WCF), str(start_configuration), str(group), str(return_full_configuration), str(options) + ) if self._current_ik["request_id"] == request_id and self._current_ik["solutions"] is not None: solution = next(self._current_ik["solutions"], None) if solution is not None: return solution - solutions = self.iter_inverse_kinematics(frame_WCF, start_configuration, group, return_full_configuration, use_attached_tool_frame, options) + solutions = self.iter_inverse_kinematics( + frame_WCF, start_configuration, group, return_full_configuration, use_attached_tool_frame, options + ) self._current_ik["request_id"] = request_id self._current_ik["solutions"] = solutions @@ -1512,7 +1540,9 @@ def forward_kinematics(self, configuration, group=None, use_attached_tool_frame= def forward_kinematics_deprecated(self, configuration, group=None, backend=None, ee_link=None): return self.forward_kinematics(configuration, group, options=dict(solver=backend, link=ee_link)) - def plan_cartesian_motion(self, frames_WCF, start_configuration=None, group=None, use_attached_tool_frame=True, options=None): + def plan_cartesian_motion( + self, frames_WCF, start_configuration=None, group=None, use_attached_tool_frame=True, options=None + ): """Calculate a cartesian motion path (linear in tool space). Parameters @@ -1723,7 +1753,9 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op >>> tolerances_above = [math.radians(5)] * len(configuration.joint_values) >>> tolerances_below = [math.radians(5)] * len(configuration.joint_values) >>> group = robot.main_group_name - >>> goal_constraints = robot.constraints_from_configuration(configuration, tolerances_above, tolerances_below, group) + >>> goal_constraints = robot.constraints_from_configuration( + ... configuration, tolerances_above, tolerances_below, group + ... ) >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {"planner_id": "RRTConnect"}) >>> trajectory.fraction 1.0 From 0ea8d3bfbb24adec2d2b4160ce1817a0abbf5a83 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Fri, 28 Mar 2025 16:31:52 +0100 Subject: [PATCH 35/36] needs skip on every line --- src/compas_fab/robots/robot.py | 78 +++++++++++++++++++--------------- 1 file changed, 43 insertions(+), 35 deletions(-) diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 50424ef8a9..8e8b6a09d5 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -1584,23 +1584,23 @@ def plan_cartesian_motion( Examples -------- - >>> ros = RosClient() # doctest: +SKIP - >>> ros.run() - >>> robot = ros.load_robot() - >>> frames = [Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]),\ + >>> ros = RosClient() # doctest: +SKIP # doctest: +SKIP + >>> ros.run() # doctest: +SKIP + >>> robot = ros.load_robot() # doctest: +SKIP + >>> frames = [Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]),\ # doctest: +SKIP Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0])] - >>> start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000]) - >>> group = robot.main_group_name + >>> start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000]) # doctest: +SKIP + >>> group = robot.main_group_name # doctest: +SKIP >>> options = {'max_step': 0.01,\ 'jump_threshold': 1.57,\ - 'avoid_collisions': True} + 'avoid_collisions': True} # doctest: +SKIP >>> trajectory = robot.plan_cartesian_motion(frames,\ start_configuration,\ group=group,\ - options=options) - >>> len(trajectory.points) > 1 + options=options) # doctest: +SKIP + >>> len(trajectory.points) > 1 # doctest: +SKIP True - >>> ros.close() + >>> ros.close() # doctest: +SKIP """ options = options or {} max_step = options.get("max_step") @@ -1728,40 +1728,48 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op Using position and orientation constraints: - >>> ros = RosClient() # doctest: +SKIP - >>> ros.run() - >>> robot = ros.load_robot() - >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) - >>> tolerance_position = 0.001 - >>> tolerances_axes = [math.radians(1)] * 3 - >>> start_configuration = Configuration.from_revolute_values([-0.042, 4.295, 0, -3.327, 4.755, 0.0]) - >>> group = robot.main_group_name - >>> goal_constraints = robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) - >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {"planner_id": "RRTConnect"}) - >>> trajectory.fraction + >>> ros = RosClient() # doctest: +SKIP # doctest: +SKIP + >>> ros.run() # doctest: +SKIP + >>> robot = ros.load_robot() # doctest: +SKIP + >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) # doctest: +SKIP + >>> tolerance_position = 0.001 # doctest: +SKIP + >>> tolerances_axes = [math.radians(1)] * 3 # doctest: +SKIP + >>> start_configuration = Configuration.from_revolute_values( + ... [-0.042, 4.295, 0, -3.327, 4.755, 0.0] + ... ) # doctest: +SKIP + >>> group = robot.main_group_name # doctest: +SKIP + >>> goal_constraints = robot.constraints_from_frame( + ... frame, tolerance_position, tolerances_axes, group + ... ) # doctest: +SKIP + >>> trajectory = robot.plan_motion( + ... goal_constraints, start_configuration, group, {"planner_id": "RRTConnect"} + ... ) # doctest: +SKIP + >>> trajectory.fraction # doctest: +SKIP 1.0 - >>> len(trajectory.points) > 1 + >>> len(trajectory.points) > 1 # doctest: +SKIP True - >>> ros.close() + >>> ros.close() # doctest: +SKIP Using joint constraints (to the UP configuration): - >>> ros = RosClient() # doctest: +SKIP - >>> ros.run() - >>> robot = ros.load_robot() - >>> configuration = Configuration.from_revolute_values([0.0, -1.5707, 0.0, -1.5707, 0.0, 0.0]) - >>> tolerances_above = [math.radians(5)] * len(configuration.joint_values) - >>> tolerances_below = [math.radians(5)] * len(configuration.joint_values) - >>> group = robot.main_group_name + >>> ros = RosClient() # doctest: +SKIP # doctest: +SKIP + >>> ros.run() # doctest: +SKIP + >>> robot = ros.load_robot() # doctest: +SKIP + >>> configuration = Configuration.from_revolute_values([0.0, -1.5707, 0.0, -1.5707, 0.0, 0.0]) # doctest: +SKIP + >>> tolerances_above = [math.radians(5)] * len(configuration.joint_values) # doctest: +SKIP + >>> tolerances_below = [math.radians(5)] * len(configuration.joint_values) # doctest: +SKIP + >>> group = robot.main_group_name # doctest: +SKIP >>> goal_constraints = robot.constraints_from_configuration( ... configuration, tolerances_above, tolerances_below, group - ... ) - >>> trajectory = robot.plan_motion(goal_constraints, start_configuration, group, {"planner_id": "RRTConnect"}) - >>> trajectory.fraction + ... ) # doctest: +SKIP + >>> trajectory = robot.plan_motion( + ... goal_constraints, start_configuration, group, {"planner_id": "RRTConnect"} + ... ) # doctest: +SKIP + >>> trajectory.fraction # doctest: +SKIP 1.0 - >>> len(trajectory.points) > 1 + >>> len(trajectory.points) > 1 # doctest: +SKIP True - >>> ros.close() + >>> ros.close() # doctest: +SKIP """ options = options or {} path_constraints = options.get("path_constraints") From 8bcf41040ace49b1c02f3ba8aac88c9a4dd65565 Mon Sep 17 00:00:00 2001 From: Chen Kasirer Date: Wed, 2 Apr 2025 20:43:04 +0200 Subject: [PATCH 36/36] fixed some doctest skipping statements --- src/compas_fab/robots/robot.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 8e8b6a09d5..720fe5f232 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -1584,13 +1584,13 @@ def plan_cartesian_motion( Examples -------- - >>> ros = RosClient() # doctest: +SKIP # doctest: +SKIP + >>> ros = RosClient() # doctest: +SKIP >>> ros.run() # doctest: +SKIP >>> robot = ros.load_robot() # doctest: +SKIP - >>> frames = [Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]),\ # doctest: +SKIP - Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0])] + >>> frames = [Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]),\ + Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0])] # doctest: +SKIP >>> start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000]) # doctest: +SKIP - >>> group = robot.main_group_name # doctest: +SKIP + >>> group = robot.main_group_name >>> options = {'max_step': 0.01,\ 'jump_threshold': 1.57,\ 'avoid_collisions': True} # doctest: +SKIP @@ -1728,7 +1728,7 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op Using position and orientation constraints: - >>> ros = RosClient() # doctest: +SKIP # doctest: +SKIP + >>> ros = RosClient() # doctest: +SKIP >>> ros.run() # doctest: +SKIP >>> robot = ros.load_robot() # doctest: +SKIP >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) # doctest: +SKIP @@ -1752,7 +1752,7 @@ def plan_motion(self, goal_constraints, start_configuration=None, group=None, op Using joint constraints (to the UP configuration): - >>> ros = RosClient() # doctest: +SKIP # doctest: +SKIP + >>> ros = RosClient() # doctest: +SKIP >>> ros.run() # doctest: +SKIP >>> robot = ros.load_robot() # doctest: +SKIP >>> configuration = Configuration.from_revolute_values([0.0, -1.5707, 0.0, -1.5707, 0.0, 0.0]) # doctest: +SKIP